Ubuntu20.04

.gitignore

@@ -3,3 +3,6 @@ install
 log
 .git
 venv
+.vscode
+.idea
+

.vscode/c_cpp_properties.json

@@ -8,6 +8,10 @@
             "includePath": [
                 "${workspaceFoler}/**",
                 "${workspaceFolder}/install/custom_interfaces/include/custom_interfaces",
+                "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces",
+                "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces/**",
+                "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces/msg",
+                "${workspaceFolder}/src/custom_interfaces/msg",
                 "/opt/ros/humble/include/**",
                 "/home/daeyun/Desktop/github_repos/SIFM/**",
                 "/home/daeyun/Desktop/github_repos/SIFM/src/tcp_pkg/include",

common/RealSense_Utilities/MATLAB/depth_options.txt

@@ -0,0 +1,53 @@
+/device_0/sensor_0/option/Alternate_IR/description, Enable/Disable alternate IR
+/device_0/sensor_0/option/Alternate_IR/value, 0.000000
+/device_0/sensor_0/option/Apd_Temperature/description, Avalanche Photo Diode temperature
+/device_0/sensor_0/option/Apd_Temperature/value, 33.078812
+/device_0/sensor_0/option/Confidence_Threshold/description, The confidence level threshold to use to mark a pixel as valid by the depth algorithm
+/device_0/sensor_0/option/Confidence_Threshold/value, 1.000000
+/device_0/sensor_0/option/Depth_Offset/description, Offset from sensor to depth origin in millimetrers
+/device_0/sensor_0/option/Depth_Offset/value, 4.500000
+/device_0/sensor_0/option/Depth_Units/description, Number of meters represented by a single depth unit
+/device_0/sensor_0/option/Depth_Units/value, 0.000250
+/device_0/sensor_0/option/Digital_Gain/description, Change the depth digital gain to: 1 for high gain and 2 for low gain
+/device_0/sensor_0/option/Digital_Gain/value, 1.000000
+/device_0/sensor_0/option/Error_Polling_Enabled/description, Enable / disable polling of camera internal errors
+/device_0/sensor_0/option/Error_Polling_Enabled/value, 1.000000
+/device_0/sensor_0/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa
+/device_0/sensor_0/option/Frames_Queue_Size/value, 16.000000
+/device_0/sensor_0/option/Freefall_Detection_Enabled/description, When enabled (default), the sensor will turn off if a free-fall is detected
+/device_0/sensor_0/option/Freefall_Detection_Enabled/value, 1.000000
+/device_0/sensor_0/option/Global_Time_Enabled/description, Enable/Disable global timestamp
+/device_0/sensor_0/option/Global_Time_Enabled/value, 0.000000
+/device_0/sensor_0/option/Host_Performance/description, Optimize based on host performance, low power low performance host or high power high performance host
+/device_0/sensor_0/option/Host_Performance/value, 0.000000
+/device_0/sensor_0/option/Humidity_Temperature/description, Humidity temperature
+/device_0/sensor_0/option/Humidity_Temperature/value, 35.964371
+/device_0/sensor_0/option/Inter_Cam_Sync_Mode/description, Enable multi-camera hardware synchronization mode (disabled on startup); not compatible with free-fall detection
+/device_0/sensor_0/option/Inter_Cam_Sync_Mode/value, 0.000000
+/device_0/sensor_0/option/Invalidation_Bypass/description, Enable/disable pixel invalidation
+/device_0/sensor_0/option/Invalidation_Bypass/value, 0.000000
+/device_0/sensor_0/option/LDD_temperature/description, Laser Driver temperature
+/device_0/sensor_0/option/LDD_temperature/value, 41.458199
+/device_0/sensor_0/option/Laser_Power/description, Power of the laser emitter, with 0 meaning projector off
+/device_0/sensor_0/option/Laser_Power/value, 100.000000
+/device_0/sensor_0/option/Ma_Temperature/description, DSP controller temperature
+/device_0/sensor_0/option/Ma_Temperature/value, 35.222988
+/device_0/sensor_0/option/Mc_Temperature/description, Mems Controller temperature
+/device_0/sensor_0/option/Mc_Temperature/value, 34.228119
+/device_0/sensor_0/option/Min_Distance/description, Minimal distance to the target (in mm)
+/device_0/sensor_0/option/Min_Distance/value, 490.000000
+/device_0/sensor_0/option/Noise_Estimation/description, Noise estimation
+/device_0/sensor_0/option/Noise_Estimation/value, 1242.000000
+/device_0/sensor_0/option/Noise_Filtering/description, Control edges and background noise
+/device_0/sensor_0/option/Noise_Filtering/value, 4.000000
+/device_0/sensor_0/option/Post_Processing_Sharpening/description, Changes the amount of sharpening in the post-processed image
+/device_0/sensor_0/option/Post_Processing_Sharpening/value, 1.000000
+/device_0/sensor_0/option/Pre_Processing_Sharpening/description, Changes the amount of sharpening in the pre-processed image
+/device_0/sensor_0/option/Pre_Processing_Sharpening/value, 0.000000
+/device_0/sensor_0/option/Receiver_Gain/description, Changes the exposure time of Avalanche Photo Diode in the receiver
+/device_0/sensor_0/option/Receiver_Gain/value, 9.000000
+/device_0/sensor_0/option/Sensor_Mode/description, Notify the sensor about the intended streaming mode. Required for preset 
+/device_0/sensor_0/option/Sensor_Mode/value, 2.000000
+/device_0/sensor_0/option/Visual_Preset/description, Preset to calibrate the camera to environment ambient, no ambient or low ambient.
+/device_0/sensor_0/option/Visual_Preset/value, 4.000000
+/device_0/sensor_0/post_processing, Temporal Filter

common/RealSense_Utilities/MATLAB/get_options_from_rosbag.m

@@ -0,0 +1,40 @@
+bag = rosbag("C:\Users\35840\Documents\20211217_204044.bag");
+topics = bag.AvailableTopics;
+
+% Get only the option topics
+depth_sensor_option_topics = topics(14:66,:).Properties.RowNames;
+rgb_sensor_option_topics = topics(73:105,:).Properties.RowNames;
+imu_sensor_option_topics = topics(117:122,:).Properties.RowNames;
+
+% Put options data in cells
+depth_options = options_to_cell(bag, depth_sensor_option_topics)
+rgb_options = options_to_cell(bag, rgb_sensor_option_topics)
+imu_options = options_to_cell(bag, imu_sensor_option_topics)
+
+cell_to_file(depth_options, 'depth_options.txt')
+cell_to_file(rgb_options, 'rgb_options.txt')
+cell_to_file(imu_options, 'imu_options.txt')
+function cell_to_file(cell, file_name)
+    file = fopen(file_name,'w')
+    for index = 1 : numel(cell) / 2
+        disp(index)
+        if mod(index,2) == 0
+            fprintf(file,'%s, %f\n',cell{index,1},cell{index,2});
+        else
+            fprintf(file,'%s, %s\n',cell{index,1},cell{index,2});
+        end
+    end
+    fclose(file);
+end
+
+function options = options_to_cell(bag, topics)
+    for index = 1 : numel(topics)
+        m = readMessages(select(bag,'Topic',topics{index}),'DataFormat','struct');
+        options{index,1} = topics{index};
+        if mod(index,2) == 0
+            options{index,2} = m{1,1}.Data;
+        else
+            options{index,2} = m{1,1}.Data;
+        end
+    end
+end

common/RealSense_Utilities/MATLAB/imu_options.txt

@@ -0,0 +1,6 @@
+/device_0/sensor_2/option/Enable_Motion_Correction/description, Enable/Disable Automatic Motion Data Correction
+/device_0/sensor_2/option/Enable_Motion_Correction/value, 1.000000
+/device_0/sensor_2/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa
+/device_0/sensor_2/option/Frames_Queue_Size/value, 16.000000
+/device_0/sensor_2/option/Global_Time_Enabled/description, Enable/Disable global timestamp
+/device_0/sensor_2/option/Global_Time_Enabled/value, 0.000000

common/RealSense_Utilities/MATLAB/rgb_options.txt

@@ -0,0 +1,33 @@
+/device_0/sensor_1/option/Auto_Exposure_Priority/description, Restrict Auto-Exposure to enforce constant FPS rate. Turn ON to remove the restrictions (may result in FPS drop)
+/device_0/sensor_1/option/Auto_Exposure_Priority/value, 1.000000
+/device_0/sensor_1/option/Backlight_Compensation/description, Enable / disable backlight compensation
+/device_0/sensor_1/option/Backlight_Compensation/value, 128.000000
+/device_0/sensor_1/option/Brightness/description, UVC image brightness
+/device_0/sensor_1/option/Brightness/value, 0.000000
+/device_0/sensor_1/option/Contrast/description, UVC image contrast
+/device_0/sensor_1/option/Contrast/value, 50.000000
+/device_0/sensor_1/option/Enable_Auto_Exposure/description, Enable / disable auto-exposure
+/device_0/sensor_1/option/Enable_Auto_Exposure/value, 1.000000
+/device_0/sensor_1/option/Enable_Auto_White_Balance/description, Enable / disable auto-white-balance
+/device_0/sensor_1/option/Enable_Auto_White_Balance/value, 1.000000
+/device_0/sensor_1/option/Exposure/description, Controls exposure time of color camera. Setting any value will disable auto exposure
+/device_0/sensor_1/option/Exposure/value, 156.000000
+/device_0/sensor_1/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa
+/device_0/sensor_1/option/Frames_Queue_Size/value, 16.000000
+/device_0/sensor_1/option/Gain/description, UVC image gain
+/device_0/sensor_1/option/Gain/value, 256.000000
+/device_0/sensor_1/option/Global_Time_Enabled/description, Enable/Disable global timestamp
+/device_0/sensor_1/option/Global_Time_Enabled/value, 0.000000
+/device_0/sensor_1/option/Host_Performance/description, Optimize based on host performance, low power low performance host or high power high performance host
+/device_0/sensor_1/option/Host_Performance/value, 0.000000
+/device_0/sensor_1/option/Hue/description, UVC image hue
+/device_0/sensor_1/option/Hue/value, 0.000000
+/device_0/sensor_1/option/Power_Line_Frequency/description, Power Line Frequency
+/device_0/sensor_1/option/Power_Line_Frequency/value, 1.000000
+/device_0/sensor_1/option/Saturation/description, UVC image saturation setting
+/device_0/sensor_1/option/Saturation/value, 50.000000
+/device_0/sensor_1/option/Sharpness/description, UVC image sharpness setting
+/device_0/sensor_1/option/Sharpness/value, 50.000000
+/device_0/sensor_1/option/White_Balance/description, Controls white balance of color image. Setting any value will disable auto white balance
+/device_0/sensor_1/option/White_Balance/value, 4600.000000
+/device_0/sensor_1/post_processing, Decimation Filter

common/RealSense_Utilities/README.md

@@ -0,0 +1 @@
+# RealSense_Utilities

common/RealSense_Utilities/object_detection.py

@@ -0,0 +1,113 @@
+'''Program is using this code with modifications: 
+https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/distance_to_object.ipynb'''
+# TODO: 
+#   - 
+#   - 
+#   - 
+#   - 
+
+
+import cv2
+
+# Color constant for opencv
+WHITE = (255, 255, 255)
+
+class ObjectDetect:
+
+    def __init__(self, color_frame, depth_frame, depth_scale):
+        self.color_frame = color_frame
+        self.depth_frame = depth_frame
+        self.depth_scale = depth_scale
+        #self.color_intrinsics = color_intrin
+
+        self.confidence: float
+
+        # Rectangle that is drawn around the detected object
+        self.rectangle_xmin = None
+        self.rectangle_ymin = None
+        self.rectangle_xmax = None
+        self.rectangle_ymax = None
+
+
+    def detect(self):
+        # Standard OpenCV boilerplate for running the net:
+        height, width = self.color_frame.shape[:2]
+        expected = 300
+        aspect = width / height
+        resized_image = cv2.resize(self.color_frame, (round(expected * aspect), expected))
+        crop_start = round(expected * (aspect - 1) / 2)
+        crop_img = resized_image[0:expected, crop_start:crop_start+expected]
+
+        net = cv2.dnn.readNetFromCaffe("C:\\Users\\35840\\Downloads\\MobileNetSSD\\MobileNetSSD\\MobileNetSSD_deploy.prototxt", "C:\\Users\\35840\\Downloads\\MobileNetSSD\\MobileNetSSD\\MobileNetSSD_deploy.caffemodel")
+        inScaleFactor = 0.007843
+        meanVal       = 127.53
+        class_names = ("background", "aeroplane", "bicycle", "bird", "boat",
+                    "bottle", "bus", "car", "cat", "chair",
+                    "cow", "diningtable", "dog", "horse",
+                    "motorbike", "person", "pottedplant",
+                    "sheep", "sofa", "train", "tvmonitor")
+        #class_names = ("bottle", "chair", "diningtable", "person", "pottedplant", "tvmonitor")
+        blob = cv2.dnn.blobFromImage(crop_img, inScaleFactor, (expected, expected), meanVal, False)
+        net.setInput(blob, "data")
+        detections = net.forward("detection_out")
+
+        label = detections[0,0,0,1]
+        conf  = detections[0,0,0,2]
+        xmin  = detections[0,0,0,3]
+        ymin  = detections[0,0,0,4]
+        xmax  = detections[0,0,0,5]
+        ymax  = detections[0,0,0,6]
+
+        class_name = class_names[int(label)]
+
+        confidence =  str(round(conf,2))[0:4]
+
+        # cv2.rectangle(crop_img, (int(xmin * expected), int(ymin * expected)), 
+        #             (int(xmax * expected), int(ymax * expected)), (255, 255, 255), 2)
+
+        scale = height / expected
+        xmin_depth = int((xmin * expected + crop_start) * scale)
+        ymin_depth = int((ymin * expected) * scale)
+        xmax_depth = int((xmax * expected + crop_start) * scale)
+        ymax_depth = int((ymax * expected) * scale)
+
+        # Crop depth data:
+        depth = self.depth_frame[xmin_depth:xmax_depth,ymin_depth:ymax_depth].astype(float)
+
+        # Get data scale from the device and convert to meters
+        depth = depth * self.depth_scale
+        distance,_,_,_ = cv2.mean(depth)
+
+        # if class_name not in ["bottle", "chair", "diningtable", "person", "pottedplant", "tvmonitor"]:
+        #     class_name = "unknown"
+
+        self.rectangle_xmin = xmin_depth
+        self.rectangle_ymin = ymin_depth
+        self.rectangle_xmax = xmax_depth
+        self.rectangle_ymax = ymax_depth
+        self.confidence = confidence
+        self.class_name = class_name
+        self.distance = distance
+
+    def draw_rectangle(self, image):
+
+        # Rectangle variables 
+        top_left_corner = (self.rectangle_xmin, self.rectangle_ymin)
+        bottom_right_corner = (self.rectangle_xmax, self.rectangle_ymax)
+        rectangle_color = WHITE
+        rectangle_thickness = 2
+
+        # Text variables
+        text = f'{self.class_name} {self.distance:.2f} meters away'
+        text_location = (self.rectangle_xmin, self.rectangle_ymin - 5)
+        text_font = cv2.FONT_HERSHEY_COMPLEX
+        text_color = WHITE
+        text_scale = 0.5
+
+        cv2.rectangle(image,
+                      top_left_corner, bottom_right_corner,
+                      rectangle_color, rectangle_thickness)
+
+        cv2.putText(self.color_frame, text,
+                    text_location, text_font,
+                    text_scale, text_color)