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()