From 969c16fca75bb98aab47e78a880b9a17a3c4fc9c Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 9 Feb 2022 22:00:50 -0800 Subject: [PATCH] Fix setting initial preset of realsense --- stretch_core/config/HighAccuracyPreset.json | 48 +++++++++++++++++ stretch_core/launch/d435i_basic.launch | 51 ++++++++++--------- .../launch/d435i_high_resolution.launch | 16 +++--- .../launch/d435i_low_resolution.launch | 13 ++--- .../launch/stretch_all_drivers.launch | 18 +++---- stretch_core/launch/stretch_realsense.launch | 16 +++--- stretch_core/nodes/d435i_configure | 22 ++------ 7 files changed, 111 insertions(+), 73 deletions(-) create mode 100644 stretch_core/config/HighAccuracyPreset.json diff --git a/stretch_core/config/HighAccuracyPreset.json b/stretch_core/config/HighAccuracyPreset.json new file mode 100644 index 0000000..bae7ddd --- /dev/null +++ b/stretch_core/config/HighAccuracyPreset.json @@ -0,0 +1,48 @@ +{ + "ignoreSAD": "0", + "param-censusenablereg-udiameter": "9", + "param-censusenablereg-vdiameter": "9", + "param-censususize": "9", + "param-censusvsize": "9", + "param-disableraucolor": "0", + "param-disablesadcolor": "0", + "param-disablesadnormalize": "0", + "param-disablesloleftcolor": "0", + "param-disableslorightcolor": "1", + "param-lambdaad": "751", + "param-lambdacensus": "6", + "param-leftrightthreshold": "10", + "param-maxscorethreshb": "2893", + "param-medianthreshold": "796", + "param-minscorethresha": "4", + "param-neighborthresh": "108", + "param-raumine": "6", + "param-rauminn": "3", + "param-rauminnssum": "7", + "param-raumins": "2", + "param-rauminw": "2", + "param-rauminwesum": "12", + "param-regioncolorthresholdb": "0.785714", + "param-regioncolorthresholdg": "0.565558", + "param-regioncolorthresholdr": "0.985323", + "param-regionshrinku": "3", + "param-regionshrinkv": "0", + "param-robbinsmonrodecrement": "25", + "param-robbinsmonroincrement": "2", + "param-rsmdiffthreshold": "1.65625", + "param-rsmrauslodiffthreshold": "0.71875", + "param-rsmremovethreshold": "0.809524", + "param-scanlineedgetaub": "13", + "param-scanlineedgetaug": "15", + "param-scanlineedgetaur": "30", + "param-scanlinep1": "155", + "param-scanlinep1onediscon": "160", + "param-scanlinep1twodiscon": "59", + "param-scanlinep2": "190", + "param-scanlinep2onediscon": "507", + "param-scanlinep2twodiscon": "493", + "param-secondpeakdelta": "647", + "param-texturecountthresh": "0", + "param-texturedifferencethresh": "1722", + "param-usersm": "1" +} \ No newline at end of file diff --git a/stretch_core/launch/d435i_basic.launch b/stretch_core/launch/d435i_basic.launch index 68f99cc..cc2e7a2 100644 --- a/stretch_core/launch/d435i_basic.launch +++ b/stretch_core/launch/d435i_basic.launch @@ -1,50 +1,48 @@ + + - + accelerometer). --> - - - + - - - - + - + pointcloud are enabled. --> - - + - - - + true, the device will reset prior to usage. --> + + + + + + diff --git a/stretch_core/launch/d435i_high_resolution.launch b/stretch_core/launch/d435i_high_resolution.launch index ef22242..22c4d7c 100644 --- a/stretch_core/launch/d435i_high_resolution.launch +++ b/stretch_core/launch/d435i_high_resolution.launch @@ -1,16 +1,18 @@ - + + + - + + + - + diff --git a/stretch_core/launch/d435i_low_resolution.launch b/stretch_core/launch/d435i_low_resolution.launch index 495f739..6380987 100644 --- a/stretch_core/launch/d435i_low_resolution.launch +++ b/stretch_core/launch/d435i_low_resolution.launch @@ -1,17 +1,18 @@ + + - + - + + diff --git a/stretch_core/launch/stretch_all_drivers.launch b/stretch_core/launch/stretch_all_drivers.launch index e33ed3a..71809f5 100644 --- a/stretch_core/launch/stretch_all_drivers.launch +++ b/stretch_core/launch/stretch_all_drivers.launch @@ -1,13 +1,13 @@ - - - - - - - - + + + + + + + + @@ -17,7 +17,7 @@ - + diff --git a/stretch_core/launch/stretch_realsense.launch b/stretch_core/launch/stretch_realsense.launch index bafce9d..9cec51a 100644 --- a/stretch_core/launch/stretch_realsense.launch +++ b/stretch_core/launch/stretch_realsense.launch @@ -1,17 +1,17 @@ - - - - + + + + - + + + - - - + diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure index ce1399f..7abc4d6 100755 --- a/stretch_core/nodes/d435i_configure +++ b/stretch_core/nodes/d435i_configure @@ -8,23 +8,22 @@ import threading class D435iConfigureNode: def __init__(self): self.rate = 1.0 - self.visual_preset = None self.mode_lock = threading.Lock() - + def turn_on_default_mode(self): with self.mode_lock: self.locked_mode_id = 1 self.locked_mode_name = 'Default' self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name}) rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name)) - + def turn_on_high_accuracy_mode(self): with self.mode_lock: self.locked_mode_id = 3 self.locked_mode_name = 'High Accuracy' self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name}) rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name)) - + def default_mode_service_callback(self, request): self.turn_on_default_mode() return TriggerResponse( @@ -54,24 +53,11 @@ class D435iConfigureNode: Trigger, self.high_accuracy_mode_service_callback) - initial_mode = rospy.get_param('~initial_mode') - - rospy.loginfo("initial_mode = {0}".format(initial_mode)) - - if initial_mode == 'High Accuracy': - self.turn_on_high_accuracy_mode() - elif initial_mode == 'Default': - self.turn_on_default_mode() - else: - error_string = 'initial_mode = {0} not recognized. Setting to D435i to Default mode.'.format(initial_mode) - rospy.logerr(error_string) - self.turn_on_default_mode() - rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep() - + if __name__ == '__main__': try: node = D435iConfigureNode()