From e8097f181376157295e23f8f3c155f5f2f24e01c Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Wed, 31 Aug 2022 12:47:22 -0700
Subject: [PATCH] Use initial preset & reset to fix rs bugs

---
 stretch_core/config/HighAccuracyPreset.json | 48 +++++++++++++++++++++
 stretch_core/launch/d435i_basic.launch      | 10 ++++-
 stretch_core/nodes/d435i_configure          | 13 ------
 3 files changed, 57 insertions(+), 14 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..e8846f4 100644
--- a/stretch_core/launch/d435i_basic.launch
+++ b/stretch_core/launch/d435i_basic.launch
@@ -1,5 +1,8 @@
 <launch>
 
+  <arg name="rs_initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="filepath to json defining a visual preset for the depth image" />
+  <arg name="rs_initial_reset"  default="true"                                                doc="whether to reset camera before streaming, useful if the camera got into a bad state in the last session" />
+
   <!-- REDUCE RATE AND USE NUC TIME -->
   <node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
 
@@ -18,6 +21,8 @@
     <arg name="enable_infra1" value="false"/>
     <arg name="enable_infra2" value="false"/>
     <arg name="enable_accel" value="true"/>
+    <arg name="enable_color" value="true"/>
+    <arg name="enable_depth" value="true"/>
 
     <arg name="depth_width" value="$(arg depth_width)"/>
     <arg name="depth_height" value="$(arg depth_height)"/>
@@ -56,9 +61,12 @@
          properly and due to firmware issues needs to reset. If set to
          true, the device will reset prior to usage."
          https://github.com/intel-ros/realsense -->
-    <!--<arg name="initial_reset" value="true"/>-->
+    <arg name="initial_reset" value="$(arg rs_initial_reset)"/>
     
     <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
+
+    <!-- configure the depth image to the high accuracy visual preset -->
+    <arg name="json_file_path" value="$(arg rs_initial_preset)" />
   </include>
 
 </launch>
diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure
index 0df3d24..a0cefb0 100755
--- a/stretch_core/nodes/d435i_configure
+++ b/stretch_core/nodes/d435i_configure
@@ -56,19 +56,6 @@ 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()