Fix setting initial preset of realsense

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"
+}

stretch_core/launch/d435i_basic.launch

@@ -1,50 +1,48 @@
 <launch>
 
+  <arg name="initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+
   <!-- REDUCE RATE AND USE NUC TIME -->
   <node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
 
   <!-- REALSENSE D435i -->
   <include file="$(find realsense2_camera)/launch/rs_camera.launch">
-    <!-- "The D435i depth camera generates and transmits the gyro and
+    <arg name="enable_confidence" value="false"/>
+    <arg name="enable_infra1" value="false"/>
+    <arg name="enable_infra2" value="false"/>
+    <arg name="enable_infra" value="false"/>
+    <arg name="enable_color" value="true"/>
+    <arg name="enable_depth" value="true"/>
+    <arg name="enable_accel" value="true"/>
+    <arg name="enable_gyro" value="true"/>
+
+    <!-- The D435i depth camera generates and transmits the gyro and
          accelerometer samples independently, as the inertial sensors
          exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for
-         accelerometer)."
-         https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/
-         https://github.com/intel-ros/realsense
-         -->
+         accelerometer). -->
     <arg name="accel_fps" value="63"/>
     <arg name="gyro_fps" value="200"/>
     <arg name="depth_fps" value="15"/>
-    <arg name="enable_infra1" value="false"/>
-    <arg name="enable_infra2" value="false"/>
-    <arg name="enable_accel" value="true"/>
+    <arg name="color_fps" value="15"/>
 
     <arg name="depth_width" value="$(arg depth_width)"/>
     <arg name="depth_height" value="$(arg depth_height)"/>
     <arg name="color_width" value="$(arg color_width)"/>
     <arg name="color_height" value="$(arg color_height)"/>
-  
-    <arg name="color_fps" value="15"/>
 
     <!-- publish depth streams aligned to other streams -->
     <arg name="align_depth" value="true"/>
 
     <!-- publish an RGBD point cloud -->
     <arg name="filters" value="pointcloud"/>
-    
-    <!-- "tf_prefix: By default all frame's ids have the same prefix -
-         camera_. This allows changing it per camera."
-         https://github.com/intel-ros/realsense -->
+    <arg name="enable_pointcloud" value="true"/>
 
-    <!-- "enable_sync: gathers closest frames of different sensors,
+    <!-- enable_sync: gathers closest frames of different sensors,
          infra red, color and depth, to be sent with the same
          timetag. This happens automatically when such filters as
-         pointcloud are enabled."
-         https://github.com/intel-ros/realsense -->
+         pointcloud are enabled. -->
     <arg name="enable_sync" value="true"/>
-    
-    <!-- "You can have a full depth to pointcloud, coloring the
-         regions beyond the texture with zeros..." -->
+
     <!-- Set to true in order to make use of the full field of view of
          the depth image instead of being restricted to the field of
          view associated with the narrower RGB camera. Note that
@@ -52,13 +50,16 @@
          colors set to 0,0,0. -->
     <arg name="allow_no_texture_points" value="true"/>
 
-    <!-- "initial_reset: On occasions the device was not closed
+    <!-- initial_reset: On occasions the device was not closed
          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"/>-->
-    
+         true, the device will reset prior to usage. -->;
+    <arg name="initial_reset" value="true"/>
+
+    <!-- publishes a single imu message with gyro/accel filled out -->
     <!--<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 initial_preset)" />
   </include>
 
 </launch>

stretch_core/launch/d435i_high_resolution.launch

@@ -1,16 +1,18 @@
 <launch>
-  
+
+  <arg name="initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	HIGHEST RESOLUTION, but also has the highest minimum depth
-	(280mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->
+    <!-- HIGHEST RESOLUTION, but also has the highest minimum depth
+         (280mm Min-Z) below which objects generate bad noise, such as
+         when the arm and gripper are raised close to the camera. -->
     <arg name="depth_width" value="1280"/>
     <arg name="depth_height" value="720"/>
     <arg name="color_width" value="1280"/>
     <arg name="color_height" value="720"/>
+
+    <arg name="initial_preset" value="$(arg initial_preset)" />
   </include>
-  
+
 </launch>

stretch_core/launch/d435i_low_resolution.launch

@@ -1,17 +1,18 @@
 <launch>
 
+  <arg name="initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	LOWEST RESOLUTION, but also has the lowest minimum depth
-	(105mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->    
+    <!-- LOWEST RESOLUTION, but also has the lowest minimum depth
+         (105mm Min-Z) below which objects generate bad noise, such as
+         when the arm and gripper are raised close to the camera. -->
     <arg name="depth_width" value="424"/>
     <arg name="depth_height" value="240"/>
     <arg name="color_width" value="424"/>
     <arg name="color_height" value="240"/>
-    
+
+    <arg name="initial_preset" value="$(arg initial_preset)" />
   </include>
 
 </launch>

stretch_core/launch/stretch_all_drivers.launch

@@ -1,13 +1,13 @@
 <launch>
 
-  <arg name="rviz"                   default="false"         doc="whether to show Rviz" />
-  <arg name="broadcast_odom_tf"      default="true"          doc="whether to broadcast odom tf" />
-  <arg name="fail_out_of_range_goal" default="true"          doc="whether to fail trajectories with goals exceeding joint ranges" />
-  <arg name="rs_resolution"          default="high"          doc="resolution of the color/depth images ('low' or 'high')" />
-  <arg name="rs_preset"              default="High Accuracy" doc="depth image visual preset ('Default', 'High Accuracy', and more at https://dev.intelrealsense.com/docs/d400-series-visual-presets#section-preset-table)" />
-  <arg name="rs_publish_frustum_viz" default="false"         doc="whether to pub viz of cameras' frustums" />
-  <arg name="rs_publish_upright_img" default="true"          doc="whether to pub rotated upright color image" />
-  <arg name="rp_scan_mode"           default="Boost"         doc="scanning mode of the lidar ('Standard', 'Express', or 'Boost')" />
+  <arg name="rviz"                   default="false"                                               doc="whether to show Rviz" />
+  <arg name="broadcast_odom_tf"      default="true"                                                doc="whether to broadcast odom tf" />
+  <arg name="fail_out_of_range_goal" default="true"                                                doc="whether to fail trajectories with goals exceeding joint ranges" />
+  <arg name="rs_resolution"          default="high"                                                doc="resolution of the color/depth images ('low' or 'high')" />
+  <arg name="rs_initial_preset"      default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+  <arg name="rs_publish_frustum_viz" default="false"                                               doc="whether to pub viz of cameras' frustums" />
+  <arg name="rs_publish_upright_img" default="true"                                                doc="whether to pub rotated upright color image" />
+  <arg name="rp_scan_mode"           default="Boost"                                               doc="scanning mode of the lidar ('Standard', 'Express', or 'Boost')" />
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="$(arg broadcast_odom_tf)"/>
@@ -17,7 +17,7 @@
   <!-- D435i CAMERA -->
   <include file="$(find stretch_core)/launch/stretch_realsense.launch">
     <arg name="resolution" value="$(arg rs_resolution)" />
-    <arg name="preset" value="$(arg rs_preset)" />
+    <arg name="initial_preset" value="$(arg rs_initial_preset)" />
     <arg name="publish_frustum_viz" value="$(eval rs_publish_frustum_viz or rviz)" />
     <arg name="publish_upright_img" value="$(arg rs_publish_upright_img)" />
   </include>

stretch_core/launch/stretch_realsense.launch

@@ -1,17 +1,17 @@
 <launch>
 
-  <arg name="resolution"          default="high"          doc="resolution of the color/depth images ('low' or 'high')" />
-  <arg name="preset"              default="High Accuracy" doc="depth image visual preset ('Default', 'High Accuracy', and more at https://dev.intelrealsense.com/docs/d400-series-visual-presets#section-preset-table)" />
-  <arg name="publish_frustum_viz" default="false"         doc="whether to pub viz of cameras' frustums" />
-  <arg name="publish_upright_img" default="true"          doc="whether to pub rotated upright color image" />
+  <arg name="resolution"          default="high"                                                doc="resolution of the color/depth images ('low' or 'high')" />
+  <arg name="initial_preset"      default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+  <arg name="publish_frustum_viz" default="false"                                               doc="whether to pub viz of cameras' frustums" />
+  <arg name="publish_upright_img" default="true"                                                doc="whether to pub rotated upright color image" />
 
   <!-- D435i DRIVER -->
-  <include file="$(find stretch_core)/launch/d435i_$(arg resolution)_resolution.launch" />
+  <include file="$(find stretch_core)/launch/d435i_$(arg resolution)_resolution.launch">
+    <arg name="initial_preset" value="$(arg initial_preset)" />
+  </include>
 
   <!-- VISUAL PRESET CONFIGURATION -->
-  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
-    <param name="initial_mode" type="string" value="$(arg preset)"/>
-  </node>
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" />
 
   <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
   <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" if="$(arg publish_frustum_viz)" />

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