/** Dynamic reconfigure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfigure starts or changes a parameter value.
*
* @param newconfig new Config values
* @param level bit-wise OR of reconfiguration levels for all
* changed parameters (0xffffffff on initial call)
**/
void reconfig(Config &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = "camera";
std::string tf_prefix = tf::getPrefixParam(privNH_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(newconfig))
{
// update GUID string parameter
newconfig.guid = camera_name_;
}
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_->validateURL(newconfig.camera_info_url))
{
cinfo_->loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if (state_ != Driver::CLOSED) // openCamera() succeeded?
{
// configure IIDC features
if (level & Levels::RECONFIGURE_CLOSE)
{
// initialize all features for newly opened device
if (false == dev_->features_->initialize(&newconfig))
{
ROS_ERROR_STREAM("[" << camera_name_
<< "] feature initialization failure");
closeCamera(); // can't continue
}
}
else
{
// update any features that changed
dev_->features_->reconfigure(&newconfig);
}
}
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
void reconfig(swissranger_camera::SwissRangerConfig &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = camera_name_;
if( config_.frame_id != newconfig.frame_id )
{
std::string tf_prefix = tf::getPrefixParam(nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
config_.frame_id = newconfig.frame_id;
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_->validateURL(newconfig.camera_info_url))
{
cinfo_->loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if ((config_.auto_exposure != newconfig.auto_exposure) && device_open_)
{
if( newconfig.auto_exposure == 1)
{
SRNode::dev_->setAutoExposure(true);
}
else
{
SRNode::dev_->setAutoExposure(false);
}
}
if ((config_.integration_time != newconfig.integration_time) && device_open_)
{
if( newconfig.auto_exposure != 1 )
{
SRNode::dev_->setIntegrationTime(newconfig.integration_time);
}
}
if ((config_.amp_threshold != newconfig.amp_threshold) && device_open_)
{
if( newconfig.amp_threshold >= 0 )
{
SRNode::dev_->setAmplitudeThreshold(newconfig.amp_threshold);
}
else
{
SRNode::dev_->setAmplitudeThreshold(0);
}
}
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
/** Dynamic reconfigure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfigure starts or changes a parameter value.
*
* @param newconfig new Config values
* @param level bit-wise OR of reconfiguration levels for all
* changed parameters (0xffffffff on initial call)
**/
void reconfig(Config &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = "camera";
std::string tf_prefix = tf::getPrefixParam(privNH_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(newconfig))
{
// update camera name string
newconfig.camera_name = camera_name_;
}
}
///THIS IS A QUICK HACK TO GET EXPOSURE ON OUR CAMERA'S THIS DOES NOT WORK FOR ALL CAMERAS
if(config_.exposure != newconfig.exposure){
try {
cam_->set_control(0x9a0901, newconfig.exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting exposure. Exception was " << e.what());
}
}
if(config_.absolute_exposure != newconfig.absolute_exposure){
try {
cam_->set_control(0x9a0902, newconfig.absolute_exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting absolute exposure. Exception was " << e.what());
}
}
if(config_.sharpness != newconfig.sharpness){
try {
cam_->set_control(0x98091b, newconfig.sharpness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting sharpness. Exception was " << e.what());
}
}
if(config_.power_line_frequency != newconfig.power_line_frequency){
try {
cam_->set_control(0x980918, newconfig.power_line_frequency);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting powerline frequency. Exception was " << e.what());
}
}
if(config_.white_balance_temperature != newconfig.white_balance_temperature){
try {
cam_->set_control(0x98090c, newconfig.white_balance_temperature);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting white balance temperature. Exception was " << e.what());
}
}
if(config_.gain != newconfig.gain){
try {
cam_->set_control(0x980913, newconfig.gain);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting gain. Exception was " << e.what());
}
}
if(config_.saturation != newconfig.saturation){
try {
cam_->set_control(0x980902, newconfig.saturation);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting saturation. Exception was " << e.what());
}
}
if(config_.contrast != newconfig.contrast){
try {
cam_->set_control(0x980901, newconfig.contrast);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting contrast. Exception was " << e.what());
}
}
if(config_.brightness != newconfig.brightness){
try {
cam_->set_control(0x980900, newconfig.brightness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting brightness. Exception was " << e.what());
}
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_.validateURL(newconfig.camera_info_url))
{
cinfo_.loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
// if (state_ != Driver::CLOSED) // openCamera() succeeded?
// {
// // configure IIDC features
// if (level & Levels::RECONFIGURE_CLOSE)
// {
// // initialize all features for newly opened device
// if (false == dev_->features_->initialize(&newconfig))
// {
// ROS_ERROR_STREAM("[" << camera_name_
// << "] feature initialization failure");
// closeCamera(); // can't continue
// }
// }
// else
// {
// // update any features that changed
// dev_->features_->reconfigure(&newconfig);
// }
// }
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
