From 4ee5382fe9527efd96b2268d15aa38f1b308a0b7 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 27 Sep 2024 13:55:24 +0200 Subject: [PATCH] Updates the model based on Olive Camera lenses and setup. (#60) * Updates the model based on Olive Camera lenses and setup. * Set a QoS that makes it work with little lag and little amount of lost frames. * Updates the perspective. --------- Signed-off-by: Agustin Alba Chicar --- .../fruit_detection/fruit_detection_node.py | 23 +++++++++++-------- .../assets/olive_camera/README.md | 8 +++++-- isaac_ws/simulation_ws/conf/config.yml | 11 ++++----- isaac_ws/simulation_ws/scene/scene.usda | 16 ++++++------- .../config/image_view.perspective | 16 ++++++------- 5 files changed, 41 insertions(+), 33 deletions(-) diff --git a/detection_ws/src/fruit_detection/fruit_detection/fruit_detection_node.py b/detection_ws/src/fruit_detection/fruit_detection/fruit_detection_node.py index 98d0985..b1f35d3 100644 --- a/detection_ws/src/fruit_detection/fruit_detection/fruit_detection_node.py +++ b/detection_ws/src/fruit_detection/fruit_detection/fruit_detection_node.py @@ -70,14 +70,19 @@ class FruitDetectionNode(Node): """ TARGET_ENCODING = "bgr8" - TOPIC_QOS_QUEUE_LENGTH = 10 - QOS_PROFILE = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, + TOPIC_QOS_QUEUE_LENGTH = 2 + OLIVE_CAMERA_QOS_PROFILE = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=TOPIC_QOS_QUEUE_LENGTH, + ) + PROC_QOS_PROFILE = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=TOPIC_QOS_QUEUE_LENGTH, ) RECT_COLOR = (0, 0, 255) - LOGGING_THROTTLE = 1 + LOGGING_THROTTLE = 1.0 MINIMUM_BBOX_SIZE_X = 0 MINIMUM_BBOX_SIZE_Y = 0 MAXIMUM_BBOX_SIZE_X = 640 @@ -91,7 +96,7 @@ def __init__(self) -> None: self.declare_parameter("model_path", "model.pth") self.declare_parameter("webcam_topic", "/image_raw") self.declare_parameter( - "olive_camera_topic", "/olive/camera/id02/image/compressed" + "olive_camera_topic", "/olive/camera/id01/image/compressed" ) self.declare_parameter("bbox_min_x", 60) self.declare_parameter("bbox_min_y", 60) @@ -119,20 +124,20 @@ def __init__(self) -> None: self.webcam_image_callback, FruitDetectionNode.TOPIC_QOS_QUEUE_LENGTH, ) - self.webcam_image_subscription = self.create_subscription( + self.olive_camera_image_subscription = self.create_subscription( CompressedImage, self.__olive_camera_topic, self.olive_image_callback, - FruitDetectionNode.QOS_PROFILE, + FruitDetectionNode.OLIVE_CAMERA_QOS_PROFILE, ) self.image_publisher = self.create_publisher( - Image, "/proc_image", FruitDetectionNode.TOPIC_QOS_QUEUE_LENGTH + Image, "/proc_image", FruitDetectionNode.PROC_QOS_PROFILE ) self.detections_publisher = self.create_publisher( Detection2DArray, "/detections", - FruitDetectionNode.TOPIC_QOS_QUEUE_LENGTH, + FruitDetectionNode.PROC_QOS_PROFILE, ) self.cv_bridge = CvBridge() self.device_str = "cuda" if torch.cuda.is_available() else "cpu" diff --git a/isaac_ws/simulation_ws/assets/olive_camera/README.md b/isaac_ws/simulation_ws/assets/olive_camera/README.md index e4fd90d..510fee2 100644 --- a/isaac_ws/simulation_ws/assets/olive_camera/README.md +++ b/isaac_ws/simulation_ws/assets/olive_camera/README.md @@ -2,6 +2,10 @@ ## Sensor and lens configuration information -- Olive Camera datasheet: https://www.olive-robotics.com/pdf/Product_Datasheet_OLV-CAM01-TP.pdf +- Olive Camera documentation: https://docs.olive-robotics.com/hardware/camera/camera_01_tp.html - Sensor OV2710 datasheet: https://static6.arrow.com/aropdfconversion/20a4701909ea0c3602c8573c489a64272f1c28d6/ov2710pbv1.1web.pdf -- 2.1 L210 lens datasheet: https://www.activesilicon.com/wp-content/uploads/DATASHEET-M12-lens-2.1mm-for-board-cam.pdf +- Olive Camera compatible lenses: https://docs.olive-robotics.com/hardware/camera/camera_01_tp.html#supported-camera-lenses + +For this project, we have worked with the "2.8-12mm 1:1.4 IR" and "Far-view" lenses. + +By default, the "2.8-12mm 1:1.4 IR" lens is set. diff --git a/isaac_ws/simulation_ws/conf/config.yml b/isaac_ws/simulation_ws/conf/config.yml index e448279..bd5e7bc 100644 --- a/isaac_ws/simulation_ws/conf/config.yml +++ b/isaac_ws/simulation_ws/conf/config.yml @@ -34,10 +34,9 @@ OBJECTS_POSE_CONFIG: min_rot: [-180.0, -90.0, -180.0] # Minimum rotation (Euler angles) for objects. max_rot: [180.0, 90.0, 180.0] # Maximum rotation (Euler angles) for objects. -OBJECTS_SCALE: [0.1, 0.1, 0.1] # Uniform scaling factor for all objects. +OBJECTS_SCALE: [0.15, 0.15, 0.15] # Uniform scaling factor for all objects. LIGHT_CONFIG: - light_type: distant # Type of light, "distant" for sunlight-like behavior. min_color: [0.5, 0.5, 0.5] # Minimum RGB color for light (gray). max_color: [0.9, 0.9, 0.9] # Maximum RGB color for light (almost white). min_distant_intensity: 500.0 # Minimum intensity for distant light. @@ -62,11 +61,11 @@ SDG_CAMERA: width: 640 # Camera resolution width in pixels. height: 480 # Camera resolution height in pixels. name: sdg_camera # Name for the camera object. - pos: [0.0, 0.0, 5.0] # Camera XYZ position in world coordinates. + pos: [0.0, 0.0, 4.45] # Camera XYZ position in world coordinates. rot: [0.0, -90.0, 0.0] # Camera rotation (Euler angles). - focal_length: 2.1 # Focal length of the camera lens. - focus_distance: 5.5 # Distance at which the camera is focused. - f_stop: 200 # Aperture size, affecting depth of field. + focal_length: 2.8 # Focal length of the camera lens. + focus_distance: 5.0 # Distance at which the camera is focused. + f_stop: 140 # Aperture size, affecting depth of field. horizontal_aperture: 5.856 # Horizontal size of the camera's sensor. vertical_aperture: 3.276 # Vertical size of the camera's sensor. clipping_range: [0.01, 10000000] # Near and far clipping planes for rendering. diff --git a/isaac_ws/simulation_ws/scene/scene.usda b/isaac_ws/simulation_ws/scene/scene.usda index 304a64b..55a7ced 100644 --- a/isaac_ws/simulation_ws/scene/scene.usda +++ b/isaac_ws/simulation_ws/scene/scene.usda @@ -145,7 +145,7 @@ def Xform "World" { quatf xformOp:orient = (1, 0, 0, 0) float3 xformOp:scale = (1, 1, 1) - double3 xformOp:scale:unitsResolve = (0.01, 0.01, 0.01) + double3 xformOp:scale:unitsResolve = (0.02, 0.02, 0.02) double3 xformOp:translate = (-0.1, -0.05, 0) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale", "xformOp:scale:unitsResolve"] } @@ -156,7 +156,7 @@ def Xform "World" { quatf xformOp:orient = (1, 0, 0, 0) float3 xformOp:scale = (1, 1, 1) - double3 xformOp:scale:unitsResolve = (0.009999999776482582, 0.009999999776482582, 0.009999999776482582) + double3 xformOp:scale:unitsResolve = (0.02, 0.02, 0.02) double3 xformOp:translate = (0, 0.1, 0) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale", "xformOp:scale:unitsResolve"] } @@ -167,7 +167,7 @@ def Xform "World" { quatf xformOp:orient = (1, 0, 0, 0) float3 xformOp:scale = (1, 1, 1) - double3 xformOp:scale:unitsResolve = (0.009999999776482582, 0.009999999776482582, 0.009999999776482582) + double3 xformOp:scale:unitsResolve = (0.02, 0.02, 0.02) double3 xformOp:translate = (0.1, -0.05, 0) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale", "xformOp:scale:unitsResolve"] } @@ -176,19 +176,19 @@ def Xform "World" { quatd xformOp:orient = (1, 0, 0, 0) double3 xformOp:scale = (1, 1, 1) - double3 xformOp:translate = (0, 0, 0.5) + double3 xformOp:translate = (0, 0, 0.445) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] def Camera "olive_camera" { float2 clippingRange = (0.01, 10000000) - float focalLength = 2.1 - float focusDistance = 5.5 - float fStop = 200 + float focalLength = 2.8 + float focusDistance = 5.0 + float fStop = 140 float fthetaCx = 320 float fthetaCy = 240 float fthetaHeight = 480 - float fthetaMaxFov = 135 + float fthetaMaxFov = 90 float fthetaPolyB = 0 float fthetaPolyD = 0 float fthetaWidth = 640 diff --git a/visualization_ws/config/image_view.perspective b/visualization_ws/config/image_view.perspective index 816f03f..7c21362 100644 --- a/visualization_ws/config/image_view.perspective +++ b/visualization_ws/config/image_view.perspective @@ -4,14 +4,14 @@ "mainwindow": { "keys": { "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000000000000000000077f0000043700000000000000250000077f000004370000000100000000078000000000000000250000077f00000437')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000000000000200000077f0000043700000000000000450000077f000004370000000000000000078000000000000000450000077f00000437')", "type": "repr(QByteArray.hex)", - "pretty-print": " 7 % 7 % 7" + "pretty-print": " 7 E 7 E 7" }, "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000003e9fc0100000002fc00000000000003b50000019600fffffffc0200000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000001400000252000000a500fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f007000690063005700690064006700650074010000026c000001910000006c00fffffffc000003bb000003c50000019600fffffffc0200000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000001400000253000000a500fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f007700690064006700650074010000026d000001900000010e00ffffff000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000003c9fc0100000002fc00000000000003b5000000c800fffffffc0200000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000140000023f0000005000fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000259000001840000006c00fffffffc000003bb000003c50000010200fffffffc0200000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d00610067006500560069006500770057006900640067006500740100000014000002400000005000fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f007700690064006700650074010000025a000001830000010e00ffffff000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", "type": "repr(QByteArray.hex)", - "pretty-print": " R l l Zrqt_image_view__ImageView__2__ImageViewWidget lrqt_reconfigure__Param__1___plugincontainer_top_widget 6MinimizedDockWidgetsToolbar " + "pretty-print": " ? P Y l Zrqt_image_view__ImageView__2__ImageViewWidget lrqt_reconfigure__Param__1___plugincontainer_top_widget 6MinimizedDockWidgetsToolbar " } }, "groups": { @@ -69,7 +69,7 @@ "type": "repr" }, "mouse_pub_topic": { - "repr": "'/image_raw_mouse_left'", + "repr": "'/olive/camera/id01/image/compressed_mouse_left'", "type": "repr" }, "num_gridlines": { @@ -93,7 +93,7 @@ "type": "repr" }, "topic": { - "repr": "'/image_raw'", + "repr": "'/olive/camera/id01/image/compressed'", "type": "repr" }, "zoom1": { @@ -208,7 +208,7 @@ "type": "repr" }, "selected_nodes": { - "repr": "[]", + "repr": "['/detection_node']", "type": "repr" }, "splitter": { @@ -248,7 +248,7 @@ "plugin": { "keys": { "tree_widget_header_state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff000000000000000100000000000000050100000000000000000000000620000000010000000500000064000003a1000000060101000100000000000000000600000064ffffffff000000810000000300000006000000a00000000100000003000000d900000001000000030000006200000001000000030000002b00000001000000030000019b0000000100000003000000000000000100000003000003e80000000064')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff000000000000000100000000000000050100000000000000000000000620000000010000000500000064000003a1000000060101000100000000000000000600000064ffffffff0000008100000003000000060000013300000001000000030000010600000001000000030000006200000001000000030000002b0000000100000003000000db0000000100000003000000000000000100000003000003e80000000064')", "type": "repr(QByteArray.hex)", "pretty-print": " d d" }