Skip to content

Commit 0c16e1b

Browse files
MyzharMyzhar
authored
Fix dyn params (#536)
* Default confidence value set to 50 from 80 * Fix dynamic parameters not applying default values from `common.yaml`
1 parent 3c643fc commit 0c16e1b

File tree

4 files changed

+55
-43
lines changed

4 files changed

+55
-43
lines changed

‎zed_wrapper/cfg/Zed.cfg‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ gen = ParameterGenerator()
88
group_general = gen.add_group("general")
99

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

1414
group_video = gen.add_group("video")

‎zed_wrapper/params/common.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ gain: 100 # Dynamic - work
1414
exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false
1515
auto_whitebalance: true # Dynamic
1616
whitebalance_temperature: 4200 # Dynamic - works only if `auto_whitebalance` is false
17-
depth_confidence: 80 # Dynamic
17+
depth_confidence: 50 # Dynamic
1818
point_cloud_freq: 15.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `frame_rate` value)
1919

2020
general:

‎zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -578,7 +578,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
578578
bool mCamAutoWB = true;
579579
int mCamWB = 4200;
580580

581-
int mCamDepthConfidence = 80;
581+
int mCamDepthConfidence = 50;
582582
double mPointCloudFreq = 15.;
583583

584584
double mCamImageResizeFactor = 1.0;
@@ -594,6 +594,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
594594
bool mInitOdomWithPose;
595595
bool mResetOdom = false;
596596
bool mPublishPoseCovariance = true;
597+
bool mUpdateDynParams = false;
597598

598599
// SVO recording
599600
bool mRecording = false;

‎zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp‎

Lines changed: 51 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -338,6 +338,8 @@ void ZEDWrapperNodelet::onInit() {
338338
mDynServerMutex.lock();
339339
mDynRecServer->updateConfig(config);
340340
mDynServerMutex.unlock();
341+
342+
updateDynamicReconfigure();
341343
// <---- Dynamic Reconfigure parameters
342344

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

1962+
mUpdateDynParams = false;
1963+
19601964
//NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK");
19611965
}
19621966

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

19681972
switch (param) {
1969-
// case MAT_RESIZE_FACTOR: {
1970-
// mCamMatResizeFactor = config.mat_resize_factor;
1971-
// NODELET_INFO("Reconfigure mat_resize_factor: %g", mCamMatResizeFactor);
1972-
// //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
1973-
// mDynParMutex.unlock();
1974-
1975-
// mCamDataMutex.lock();
1976-
// size_t w = static_cast<size_t>(mCamWidth * mCamMatResizeFactor);
1977-
// size_t h = static_cast<size_t>(mCamHeight * mCamMatResizeFactor);
1978-
// mMatResol = sl::Resolution(w,h);
1979-
// NODELET_DEBUG_STREAM("Data Mat size : " << mMatResol.width << "x" << mMatResol.height);
1980-
1981-
// // Update Camera Info
1982-
// fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId,
1983-
// mRightCamOptFrameId);
1984-
// fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId,
1985-
// mRightCamOptFrameId, true);
1986-
// mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is
1987-
// // the Left one (next to
1988-
// // the ZED logo)
1989-
// mRgbCamInfoRawMsg = mLeftCamInfoRawMsg;
1990-
// mCamDataMutex.unlock();
1991-
// }
1992-
// break;
1973+
// case MAT_RESIZE_FACTOR: {
1974+
// mCamMatResizeFactor = config.mat_resize_factor;
1975+
// NODELET_INFO("Reconfigure mat_resize_factor: %g", mCamMatResizeFactor);
1976+
// //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
1977+
// mDynParMutex.unlock();
1978+
1979+
// mCamDataMutex.lock();
1980+
// size_t w = static_cast<size_t>(mCamWidth * mCamMatResizeFactor);
1981+
// size_t h = static_cast<size_t>(mCamHeight * mCamMatResizeFactor);
1982+
// mMatResol = sl::Resolution(w,h);
1983+
// NODELET_DEBUG_STREAM("Data Mat size : " << mMatResol.width << "x" << mMatResol.height);
1984+
1985+
// // Update Camera Info
1986+
// fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId,
1987+
// mRightCamOptFrameId);
1988+
// fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId,
1989+
// mRightCamOptFrameId, true);
1990+
// mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is
1991+
// // the Left one (next to
1992+
// // the ZED logo)
1993+
// mRgbCamInfoRawMsg = mLeftCamInfoRawMsg;
1994+
// mCamDataMutex.unlock();
1995+
// }
1996+
// break;
19931997

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

20012005
case POINTCLOUD_FREQ:
2002-
mPointCloudFreq = config.point_cloud_freq;
2003-
NODELET_INFO("Reconfigure point cloud frequency: %g", mPointCloudFreq);
2006+
if(config.point_cloud_freq>mCamFrameRate) {
2007+
mPointCloudFreq = mCamFrameRate;
2008+
NODELET_WARN_STREAM( "'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " << mPointCloudFreq );
2009+
2010+
mUpdateDynParams = true;
2011+
} else {
2012+
mPointCloudFreq = config.point_cloud_freq;
2013+
NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq);
2014+
}
2015+
20042016
mDynParMutex.unlock();
20052017
//NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
20062018
break;
@@ -2798,49 +2810,48 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
27982810
if( !mSvoMode && mFrameCount%5 == 0 ) {
27992811
//NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK");
28002812
mDynParMutex.lock();
2801-
bool update_dyn_params = false;
28022813

28032814
int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS);
28042815
if( brightness != mCamBrightness ) {
28052816
mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness);
28062817
NODELET_DEBUG_STREAM( "mCamBrightness changed: " << mCamBrightness << " <- " << brightness);
2807-
update_dyn_params = true;
2818+
mUpdateDynParams = true;
28082819
}
28092820

28102821
int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST);
28112822
if( contrast != mCamContrast ) {
28122823
mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast);
28132824
NODELET_DEBUG_STREAM( "mCamContrast changed: " << mCamContrast << " <- " << contrast);
2814-
update_dyn_params = true;
2825+
mUpdateDynParams = true;
28152826
}
28162827

28172828
int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE);
28182829
if( hue != mCamHue ) {
28192830
mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue);
28202831
NODELET_DEBUG_STREAM( "mCamHue changed: " << mCamHue << " <- " << hue);
2821-
update_dyn_params = true;
2832+
mUpdateDynParams = true;
28222833
}
28232834

28242835
int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION);
28252836
if( saturation != mCamSaturation ) {
28262837
mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation);
28272838
NODELET_DEBUG_STREAM( "mCamSaturation changed: " << mCamSaturation << " <- " << saturation);
2828-
update_dyn_params = true;
2839+
mUpdateDynParams = true;
28292840
}
28302841

28312842
int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS);
28322843
if( sharpness != mCamSharpness ) {
28332844
mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness);
28342845
NODELET_DEBUG_STREAM( "mCamSharpness changed: " << mCamSharpness << " <- " << sharpness);
2835-
update_dyn_params = true;
2846+
mUpdateDynParams = true;
28362847
}
28372848

28382849
#if (ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION>=1)
28392850
int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA);
28402851
if( gamma != mCamGamma ) {
28412852
mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma);
28422853
NODELET_DEBUG_STREAM( "mCamGamma changed: " << mCamGamma << " <- " << gamma);
2843-
update_dyn_params = true;
2854+
mUpdateDynParams = true;
28442855
}
28452856
#endif
28462857

@@ -2854,14 +2865,14 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
28542865
if (exposure != mCamExposure) {
28552866
mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure);
28562867
NODELET_DEBUG_STREAM( "mCamExposure changed: " << mCamExposure << " <- " << exposure);
2857-
update_dyn_params = true;
2868+
mUpdateDynParams = true;
28582869
}
28592870

28602871
int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN);
28612872
if (gain != mCamGain) {
28622873
mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain);
28632874
NODELET_DEBUG_STREAM( "mCamGain changed: " << mCamGain << " <- " << gain);
2864-
update_dyn_params = true;
2875+
mUpdateDynParams = true;
28652876
}
28662877
}
28672878

@@ -2875,16 +2886,16 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
28752886
if (wb != mCamWB) {
28762887
mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB);
28772888
NODELET_DEBUG_STREAM( "mCamWB changed: " << mCamWB << " <- " << wb);
2878-
update_dyn_params = true;
2889+
mUpdateDynParams = true;
28792890
}
28802891
}
28812892
mDynParMutex.unlock();
28822893
//NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK");
2894+
}
28832895

2884-
if(update_dyn_params) {
2885-
NODELET_DEBUG( "Update Dynamic Parameters");
2886-
updateDynamicReconfigure();
2887-
}
2896+
if(mUpdateDynParams) {
2897+
NODELET_DEBUG( "Update Dynamic Parameters");
2898+
updateDynamicReconfigure();
28882899
}
28892900
// <---- Camera Settings
28902901

0 commit comments

Comments
 (0)