Skip to content

Commit

Permalink
Fix dyn params (#536)
Browse files Browse the repository at this point in the history 
* Default confidence value set to 50 from 80
* Fix dynamic parameters not applying default values from `common.yaml`
  • Loading branch information
@Myzhar
Myzhar authored Mar 6, 2020
1 parent 3c643fc commit 0c16e1b
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 43 deletions.
2 changes: 1 addition & 1 deletion zed_wrapper/cfg/Zed.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ gen = ParameterGenerator()
group_general = gen.add_group("general")

group_depth = gen.add_group("depth")
group_depth.add("depth_confidence", int_t, 1, "Depth onfidence threshold, the lower the better", 80, 1, 100)
group_depth.add("depth_confidence", int_t, 1, "Depth onfidence threshold, the lower the better", 50, 1, 100)
group_depth.add("point_cloud_freq", double_t, 2, "Point cloud frequency", 15.0, 0.1, 60.0);

group_video = gen.add_group("video")
Expand Down
2 changes: 1 addition & 1 deletion zed_wrapper/params/common.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ gain: 100 # Dynamic - work
exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false
auto_whitebalance: true # Dynamic
whitebalance_temperature: 4200 # Dynamic - works only if `auto_whitebalance` is false
depth_confidence: 80 # Dynamic
depth_confidence: 50 # Dynamic
point_cloud_freq: 15.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `frame_rate` value)

general:
Expand Down
3 changes: 2 additions & 1 deletion zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -578,7 +578,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
bool mCamAutoWB = true;
int mCamWB = 4200;

int mCamDepthConfidence = 80;
int mCamDepthConfidence = 50;
double mPointCloudFreq = 15.;

double mCamImageResizeFactor = 1.0;
Expand All @@ -594,6 +594,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
bool mInitOdomWithPose;
bool mResetOdom = false;
bool mPublishPoseCovariance = true;
bool mUpdateDynParams = false;

// SVO recording
bool mRecording = false;
Expand Down
91 changes: 51 additions & 40 deletions zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,8 @@ void ZEDWrapperNodelet::onInit() {
mDynServerMutex.lock();
mDynRecServer->updateConfig(config);
mDynServerMutex.unlock();

updateDynamicReconfigure();
// <---- Dynamic Reconfigure parameters

// Create all the publishers
Expand Down Expand Up @@ -1957,6 +1959,8 @@ void ZEDWrapperNodelet::updateDynamicReconfigure() {
mDynRecServer->updateConfig(config);
mDynServerMutex.unlock();

mUpdateDynParams = false;

//NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK");
}

Expand All @@ -1966,30 +1970,30 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_wrapper::ZedConfig& config, ui
DynParams param = static_cast<DynParams>(level);

switch (param) {
// case MAT_RESIZE_FACTOR: {
// mCamMatResizeFactor = config.mat_resize_factor;
// NODELET_INFO("Reconfigure mat_resize_factor: %g", mCamMatResizeFactor);
// //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
// mDynParMutex.unlock();

// mCamDataMutex.lock();
// size_t w = static_cast<size_t>(mCamWidth * mCamMatResizeFactor);
// size_t h = static_cast<size_t>(mCamHeight * mCamMatResizeFactor);
// mMatResol = sl::Resolution(w,h);
// NODELET_DEBUG_STREAM("Data Mat size : " << mMatResol.width << "x" << mMatResol.height);

// // Update Camera Info
// fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId,
// mRightCamOptFrameId);
// fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId,
// mRightCamOptFrameId, true);
// mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is
// // the Left one (next to
// // the ZED logo)
// mRgbCamInfoRawMsg = mLeftCamInfoRawMsg;
// mCamDataMutex.unlock();
// }
// break;
// case MAT_RESIZE_FACTOR: {
// mCamMatResizeFactor = config.mat_resize_factor;
// NODELET_INFO("Reconfigure mat_resize_factor: %g", mCamMatResizeFactor);
// //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
// mDynParMutex.unlock();

// mCamDataMutex.lock();
// size_t w = static_cast<size_t>(mCamWidth * mCamMatResizeFactor);
// size_t h = static_cast<size_t>(mCamHeight * mCamMatResizeFactor);
// mMatResol = sl::Resolution(w,h);
// NODELET_DEBUG_STREAM("Data Mat size : " << mMatResol.width << "x" << mMatResol.height);

// // Update Camera Info
// fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId,
// mRightCamOptFrameId);
// fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId,
// mRightCamOptFrameId, true);
// mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is
// // the Left one (next to
// // the ZED logo)
// mRgbCamInfoRawMsg = mLeftCamInfoRawMsg;
// mCamDataMutex.unlock();
// }
// break;

case CONFIDENCE:
mCamDepthConfidence = config.depth_confidence;
Expand All @@ -1999,8 +2003,16 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_wrapper::ZedConfig& config, ui
break;

case POINTCLOUD_FREQ:
mPointCloudFreq = config.point_cloud_freq;
NODELET_INFO("Reconfigure point cloud frequency: %g", mPointCloudFreq);
if(config.point_cloud_freq>mCamFrameRate) {
mPointCloudFreq = mCamFrameRate;
NODELET_WARN_STREAM( "'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " << mPointCloudFreq );

mUpdateDynParams = true;
} else {
mPointCloudFreq = config.point_cloud_freq;
NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq);
}

mDynParMutex.unlock();
//NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
break;
Expand Down Expand Up @@ -2798,49 +2810,48 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
if( !mSvoMode && mFrameCount%5 == 0 ) {
//NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK");
mDynParMutex.lock();
bool update_dyn_params = false;

int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS);
if( brightness != mCamBrightness ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness);
NODELET_DEBUG_STREAM( "mCamBrightness changed: " << mCamBrightness << " <- " << brightness);
update_dyn_params = true;
mUpdateDynParams = true;
}

int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST);
if( contrast != mCamContrast ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast);
NODELET_DEBUG_STREAM( "mCamContrast changed: " << mCamContrast << " <- " << contrast);
update_dyn_params = true;
mUpdateDynParams = true;
}

int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE);
if( hue != mCamHue ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue);
NODELET_DEBUG_STREAM( "mCamHue changed: " << mCamHue << " <- " << hue);
update_dyn_params = true;
mUpdateDynParams = true;
}

int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION);
if( saturation != mCamSaturation ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation);
NODELET_DEBUG_STREAM( "mCamSaturation changed: " << mCamSaturation << " <- " << saturation);
update_dyn_params = true;
mUpdateDynParams = true;
}

int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS);
if( sharpness != mCamSharpness ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness);
NODELET_DEBUG_STREAM( "mCamSharpness changed: " << mCamSharpness << " <- " << sharpness);
update_dyn_params = true;
mUpdateDynParams = true;
}

#if (ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION>=1)
int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA);
if( gamma != mCamGamma ) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma);
NODELET_DEBUG_STREAM( "mCamGamma changed: " << mCamGamma << " <- " << gamma);
update_dyn_params = true;
mUpdateDynParams = true;
}
#endif

Expand All @@ -2854,14 +2865,14 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
if (exposure != mCamExposure) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure);
NODELET_DEBUG_STREAM( "mCamExposure changed: " << mCamExposure << " <- " << exposure);
update_dyn_params = true;
mUpdateDynParams = true;
}

int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN);
if (gain != mCamGain) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain);
NODELET_DEBUG_STREAM( "mCamGain changed: " << mCamGain << " <- " << gain);
update_dyn_params = true;
mUpdateDynParams = true;
}
}

Expand All @@ -2875,16 +2886,16 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
if (wb != mCamWB) {
mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB);
NODELET_DEBUG_STREAM( "mCamWB changed: " << mCamWB << " <- " << wb);
update_dyn_params = true;
mUpdateDynParams = true;
}
}
mDynParMutex.unlock();
//NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK");
}

if(update_dyn_params) {
NODELET_DEBUG( "Update Dynamic Parameters");
updateDynamicReconfigure();
}
if(mUpdateDynParams) {
NODELET_DEBUG( "Update Dynamic Parameters");
updateDynamicReconfigure();
}
// <---- Camera Settings

Expand Down

0 comments on commit 0c16e1b

Please sign in to comment.