add adaptive topic name based on camera name, add disparity parameters to launch file, streamline postprocess

Author: IrvingF7

README.md

@@ -57,7 +57,9 @@ There is no need to launch the RealSense ROS wrapper separately, as they are int
 ### Parameters
 There are many parameters you can tune that come with RealSense, but some of the most important ones are:
 
-1. `camera_namespace` and `camera_name`: the topic name of your camera in ROS2 will be in the form of `camera_namespace/camera_name/xxx
+1. `camera_namespace` and `camera_name`: the topic name of your camera in ROS2 will be in the form of `camera_namespace/camera_name/xxx`
+    a. The convention I am using is that I set `camera_name` to be the model name. 
+    **Note** that if you use D405, please put the string `D405` somewhere in the `camera_name`, as some logic later on depends on it. This is due to some quirkiness of D405 specifically.
 
 2. `save_folder`: indicates where to save the `.png` of the depth and color images.
 

realsense_capture/launch/realsense_sys_launch.py

@@ -28,8 +28,8 @@
 
 configurable_parameters = [{'name': 'camera_namespace',             'default': 'xArm6', 'description': 'namespace for camera'},
                            {'name': 'camera_name',                  'default': 'D405', 'description': 'camera unique name'},
-                           {'name': 'save_folder',                  'default': '/home/zf540/Desktop/save_folder/', 'description': 'folder to save images'},
-                           {'name': 'json_path',                    'default': '/home/zf540/Desktop/save_folder/realsense_transform.json', 'description': 'path to the logging json file. This is different from RealSense JSON parameter. If empty, no logging will be done'},
+                           {'name': 'save_folder',                  'default': '/home/irving/Desktop/save_folder', 'description': 'folder to save images'},
+                           {'name': 'json_path',                    'default': '/home/irving/Desktop/save_folder/realsense_transform.json', 'description': 'path to the logging json file. This is different from RealSense JSON parameter. If empty, no logging will be done'},
                            {'name': 'serial_no',                    'default': "''", 'description': 'choose device by serial number'},
                            {'name': 'usb_port_id',                  'default': "''", 'description': 'choose device by usb port id'},
                            {'name': 'device_type',                  'default': "''", 'description': 'choose device by type'},
@@ -41,15 +41,15 @@
                            {'name': 'log_level',                    'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
                            {'name': 'output',                       'default': 'screen', 'description': 'pipe node output [screen|log]'},
                            {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
-                           {'name': 'rgb_camera.color_profile',     'default': '1280x720x30', 'description': 'color stream profile'},
+                           {'name': 'rgb_camera.color_profile',     'default': '640x480x15', 'description': 'color stream profile'},
                            {'name': 'rgb_camera.color_format',      'default': 'RGB8', 'description': 'color stream format'},
                            {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'},
                            {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
                            {'name': 'enable_infra',                 'default': 'false', 'description': 'enable infra0 stream'},
                            {'name': 'enable_infra1',                'default': 'false', 'description': 'enable infra1 stream'},
                            {'name': 'enable_infra2',                'default': 'false', 'description': 'enable infra2 stream'},
-                           {'name': 'depth_module.color_profile',   'default': '1280x720x30', 'description': 'depth stream color profile'},
-                           {'name': 'depth_module.depth_profile',   'default': '1280x720x30', 'description': 'depth stream profile'},
+                           {'name': 'depth_module.color_profile',   'default': '640x480x15', 'description': 'depth stream color profile'},
+                           {'name': 'depth_module.depth_profile',   'default': '640x480x15', 'description': 'depth stream profile'},
                            {'name': 'depth_module.depth_format',    'default': 'Z16', 'description': 'depth stream format'},
                            {'name': 'depth_module.infra_profile',   'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'},
                            {'name': 'depth_module.infra_format',    'default': 'RGB8', 'description': 'infra0 stream format'},
@@ -86,7 +86,8 @@
                            {'name': 'decimation_filter.enable',     'default': 'true', 'description': 'enable_decimation_filter'},
                            {'name': 'spatial_filter.enable',        'default': 'true', 'description': 'enable_spatial_filter'},
                            {'name': 'temporal_filter.enable',       'default': 'true', 'description': 'enable_temporal_filter'},
-                           {'name': 'disparity_filter.enable',      'default': 'false', 'description': 'enable_disparity_filter'},
+                           {'name': 'disparity_filter.enable',      'default': 'true', 'description': 'enable_disparity_filter'},
+                           {'name': 'disparity_to_depth.enable',      'default': 'true', 'description': 'enable_disparity_filter'},
                            {'name': 'hole_filling_filter.enable',   'default': 'false', 'description': 'enable_hole_filling_filter'},
                            {'name': 'hdr_merge.enable',             'default': 'true', 'description': 'hdr_merge filter enablement flag'},
                            {'name': 'wait_for_device_timeout',      'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},

realsense_capture/realsense_capture/realsense_image_client.py

@@ -1,3 +1,4 @@
+from pydoc import cli
 from realsense_interface_msg.srv import TakeImage
 
 from sensor_msgs.msg import Joy
@@ -120,52 +121,50 @@ def postprocess(self, img, modality):
 
         encoded_img = self.cvbridge.imgmsg_to_cv2(img_msg=img, 
                                                   desired_encoding='passthrough')
-        # color the log message in blue
-        color_start = '\033[94m'
-        color_reset = '\033[0m'
-        
-        if self.save_folder != '':
-            if modality == 'rgb':
-                cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.rgb_count}.png'), 
-                            cv2.cvtColor(encoded_img, cv2.COLOR_RGB2BGR))
-            else:
-                cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.depth_count}.png'), 
-                            encoded_img)
-            self.get_logger().info(f'{color_start}{modality} image saved{color_reset}')
-        else:
-            self.get_logger().info(f'{color_start}{modality} image captured, not saved{color_reset}')
-        
+
         if modality == 'rgb':
+            # save the image
+            self._log_image(encoded_img, modality)
+
             # publish the image
             self.rgb_publisher.publish(img)
 
             '''
             only update json with RGB. We treat depth as an accessory
             '''
-            # update the json dict
-            if self.json_path != '':
-                self._json_update()
-
-                # overwrite the JSON file if there is one. Not sure if this is the best way to do it
-                # potentially we can just keep the json_dict in memory and dump it at the end of the program
-                # but this is a good way to keep the json file updated in case the program cannot exit as expected
-                with open(self.json_path, 'wt') as f:
-                    json.dump(self.json_dict, f)
-                
-                # color the log message in green
-                color_start = '\033[92m'
-                color_reset = '\033[0m'
-                self.get_logger().info(f'{color_start}JSON file updated{color_reset}')
+            self._json_update()
 
             # only increase the count with RGB so we don't double count
             self.rgb_count += 1
 
         elif modality == 'depth':
+            self._log_image(encoded_img, modality)
             self.depth_publisher.publish(img)
             self.depth_count += 1
 
         return encoded_img
 
+    def _log_image(self, img, modality):
+        '''
+        Save the image to disk
+        '''
+        # color the save log message in blue
+        save_color_start = '\033[94m'
+        save_color_reset = '\033[0m'
+
+        if self.save_folder != '':
+            if modality == 'rgb':
+                cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.rgb_count}.png'), 
+                            cv2.cvtColor(img, cv2.COLOR_RGB2BGR))
+                np.save(os.path.join(self.save_folder, modality, f'{modality}_{self.rgb_count}.npy'), img)
+            else:
+                cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.depth_count}.png'), 
+                            img)
+                np.save(os.path.join(self.save_folder, modality, f'{modality}_{self.depth_count}.npy'), img)
+            self.get_logger().info(f'{save_color_start}{modality} image saved{save_color_reset}')
+        else:
+            self.get_logger().info(f'{save_color_start}{modality} image captured, not saved{save_color_reset}')
+
     def depth_postprocess(self, img):
         '''
         I didn't use this. Not sure if normalizing is necessary anymore after I started to use cv_bridge. But keep it just in case
@@ -221,6 +220,21 @@ def _json_setup(self, calibration_path):
                                                             [0.0, 0.0, 1.0, 0.0]], dtype=np.float64).tolist()
 
     def _json_update(self):
+
+        # color the json log message in green
+        json_color_start = '\033[92m'
+        json_color_reset = '\033[0m'
+
+        if self.json_path != '':
+            self._json_update_helper()
+            # overwrite the JSON file if there is one. Not sure if this is the best way to do it
+            # potentially we can just keep the json_dict in memory and dump it at the end of the program
+            # but this is a good way to keep the json file updated in case the program cannot exit as expected
+            with open(self.json_path, 'wt') as f:
+                json.dump(self.json_dict, f)
+            self.get_logger().info(f'{json_color_start}JSON file updated{json_color_reset}')
+
+    def _json_update_helper(self):
         '''
         Update the json dict with the latest transform
         '''

realsense_capture/realsense_capture/realsense_image_server.py

@@ -1,3 +1,4 @@
+from calendar import c
 import rclpy
 from rclpy.node import Node
 from rclpy.callback_groups import ReentrantCallbackGroup
@@ -24,10 +25,16 @@ def __init__(self):
         self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value
 
         ############################ Subscriber Setup ########################################
+        # The topic name is different for D405 due to the fact that D405 reconstructs RGB from depth
+        if 'D405' in self.camera_name:
+            rgb_topic_name = 'image_rect_raw'
+        else:
+            rgb_topic_name = 'image_raw'
+
         # Subscribe to rgb
         self.rgb_sub = self.create_subscription(
             msg_type = Image, 
-            topic = f'/{self.camere_namespace}/{self.camera_name}/color/image_rect_raw', 
+            topic = f'/{self.camere_namespace}/{self.camera_name}/color/{rgb_topic_name}', 
             callback = functools.partial(self.sub_image_callback, 'rgb'), 
             qos_profile = 10,
             callback_group = multithread_group)