Fix high latency, CPU, memory usage (#61)

* Moved expanded gripper into a service, and other changes to reduce lag * Filtered the pointcloud before deprojecting the point * Reduced nav cam FPS, add nav cam logs * Add a CycloneDDS config * Remove unnecessary realsense streams * Revert to running separate nodes per video stream * Move processing images out of callbacks, downsize pointclouds * create and revoke object URLs for images * Reduce num threads * Removed CycloneDDS config * Reenable realsense aligned depth to color * Removed unnecessary expanded gripper view code * only launch move_to_pregrasp node if Stretch has dex wrist 3 * updated image lock for beta gripper camera * updated color conversion for beta teleop gripper camera; removed unused import * Avoid base collision for horizontal grasp near floor * Abort click-to-pregrasp on collision * Undo effort-threshold based collision detection * Switch to /stretch/joint_states * Prevent markdown precommit from changing PR template * Filter pointcloud before downsampling, to prevent overflow with too small leaf sizes * Add a TODO to remove effort thresholding from web app --------- Co-authored-by: Vinitha Ranganeni <[email protected]>
hello-robot · Jul 4, 2024 · 0dce880 · 0dce880
1 parent bda5a67
commit 0dce880
Show file tree

Hide file tree

Showing 19 changed files with 717 additions and 321 deletions.
diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md
@@ -10,8 +10,8 @@
 
 From the top-level of this repository, run:
 
-- \[ \] `pre-commit run --all-files`
+- [ ] `pre-commit run --all-files`
 
 # To merge
 
-- \[ \] `Squash & Merge`
+- [ ] `Squash & Merge`
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -59,6 +59,7 @@ repos:
     rev: 0.7.16
     hooks:
       - id: mdformat
+        exclude: \.github/.*  # Exclude files in the .github directory
 
   # Codespell checks the code for common misspellings
   - repo: https://github.com/codespell-project/codespell

diff --git a/action/MoveToPregrasp.action b/action/MoveToPregrasp.action
@@ -22,6 +22,7 @@ uint8 STATUS_TIMEOUT = 3
 uint8 STATUS_GOAL_NOT_REACHABLE = 4
 uint8 STATUS_DEPROJECTION_FAILURE = 5
 uint8 STATUS_STRETCH_DRIVER_FAILURE = 6
+uint8 STATUS_JOINTS_IN_COLLISION = 7
 
 uint8 status
 ---

diff --git a/launch/multi_camera.launch.py b/launch/multi_camera.launch.py
@@ -103,12 +103,9 @@
     'depth_module.infra_profile': D435_RESOLUTION,
     'rgb_camera.profile': D435_RESOLUTION,
     'rgb_camera.color_profile': D435_RESOLUTION,
-    'enable_infra1': 'true',
-    'gyro_fps': '200',
-    'accel_fps': '100',
-    'enable_gyro': 'true',
-    'enable_accel': 'true',
     'pointcloud.enable': 'true',
+    'pointcloud.stream_filter': '',
+    'pointcloud.stream_index_filter': '',
     'enable_sync': 'true',
     'align_depth.enable': 'true',
     'initial_reset': 'true',
@@ -123,14 +120,12 @@
     'depth_module.infra_profile': D405_RESOLUTION,
     'rgb_camera.profile': D405_RESOLUTION,
     'depth_module.color_profile': D405_RESOLUTION,
-    'gyro_fps': '200',
-    'accel_fps': '100',
-    'enable_gyro': 'true',
-    'enable_accel': 'true',
     'pointcloud.enable': 'true',
+    'pointcloud.stream_filter': '',
+    'pointcloud.stream_index_filter': '',
     'enable_sync': 'true',
-    'align_depth.enable': 'true',
-    # 'initial_reset': 'true', # TODO: Look into whether this should be True!
+    'align_depth.enable': 'false',
+    'initial_reset': 'true',
     'allow_no_texture_points': 'true',
 }
 # fmt: on
@@ -237,6 +232,22 @@ def generate_launch_description():
         }
     )
 
+    # Add RGB compression parameters
+    d435_params.append(
+        {
+            "name": f"{d435_parameter_overrides['camera_name']}.color.image_rect_raw.png_level",
+            "default": "1",
+            "description": "Compression level for the color stream",
+        }
+    )
+    d405_params.append(
+        {
+            "name": f"{d405_parameter_overrides['camera_name']}.color.image_rect_raw.png_level",
+            "default": "1",
+            "description": "Compression level for the color stream",
+        }
+    )
+
     # Map each camera to a suffix
     params_to_suffix = [
         (d435_params, "1", []),

diff --git a/launch/navigation_camera.launch.py b/launch/navigation_camera.launch.py
@@ -1,44 +1,40 @@
 from launch_ros.actions import Node
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
-    return LaunchDescription(
-        [
-            Node(
-                package="usb_cam",
-                executable="usb_cam_node_exe",
-                name="navigation_camera",
-                output="screen",
-                parameters=[
-                    {"video_device": "/dev/hello-navigation-camera"},
-                    {"image_width": 800},
-                    {"image_height": 600},
-                    {"framerate": 15.0},
-                    {"pixel_format": "mjpeg2rgb"},
-                    {"brightness": 10},
-                    {"contrast": 30},
-                    {"hue": 0},
-                    {"saturation": 80},
-                    {"sharpness": 3},
-                    {"gamma": 80},
-                    {"exposure_auto": True},
-                    # {'exposure': 150},
-                    {"white_balance_temperature_auto": True},
-                    # {'white_balance': 4250},
-                    {"gain": 10},
-                    {"camera_frame_id": "navigation_camera"},
-                    {"camera_name": "navigation_camera"},
-                    {"io_method": "mmap"},
-                ],
-                remappings=[
-                    ("/image_raw", "/navigation_camera/image_raw"),
-                    # usb_cam does not publish compressed images:
-                    # https://github.com/ros-drivers/usb_cam/blob/52dd75fb78ae608a2c40eea60f6e8eac673b91e8/src/ros2/usb_cam_node.cpp#L50
-                    # ("/image_raw/compressed", "/navigation_camera/image_raw/compressed")
-                ],
-            ),
-            # You can add more nodes or configurations here if needed.
-        ]
+    print_debug_arg = DeclareLaunchArgument(
+        "print_debug", default_value="False", description="Print debug info"
     )
+    camera_params = [
+        {
+            "camera_port": "/dev/hello-nav-head-camera",
+            "publish_topic": "/navigation_camera/image_raw",
+            "print_debug": LaunchConfiguration("print_debug"),
+            # Properties
+            "format": "MJPG",
+            "size": [800, 600],
+            "fps": 15,
+            "brightness": 10,
+            "contrast": 30,
+            "saturation": 80,
+            "hue": 0,
+            "gamma": 80,
+            "gain": 10,
+            "white_balence_temp": 4600,
+            "sharpness": 3,
+            "backlight": 1,
+        }
+    ]
+    usb_cam = Node(
+        package="stretch_core",
+        executable="usb_cam",
+        emulate_tty=True,
+        output="screen",
+        parameters=camera_params,
+        name="navigation_camera",
+    )
+    return LaunchDescription([print_debug_arg, usb_cam])
diff --git a/launch/web_interface.launch.py b/launch/web_interface.launch.py
@@ -304,7 +304,11 @@ def generate_launch_description():
                     IncludeLaunchDescription(
                         PythonLaunchDescriptionSource(
                             PathJoinSubstitution(
-                                [core_package, "launch", "navigation_camera.launch.py"]
+                                [
+                                    teleop_interface_package,
+                                    "launch",
+                                    "navigation_camera.launch.py",
+                                ]
                             )
                         )
                     )
@@ -517,13 +521,14 @@ def generate_launch_description():
     # )
 
     # Move To Pre-grasp Action Server
-    move_to_pregrasp_node = Node(
-        package="stretch_web_teleop",
-        executable="move_to_pregrasp.py",
-        output="screen",
-        arguments=[LaunchConfiguration("params")],
-        parameters=[],
-    )
-    ld.add_action(move_to_pregrasp_node)
+    if stretch_tool == "eoa_wrist_dw3_tool_sg3":
+        move_to_pregrasp_node = Node(
+            package="stretch_web_teleop",
+            executable="move_to_pregrasp.py",
+            output="screen",
+            arguments=[LaunchConfiguration("params")],
+            parameters=[],
+        )
+        ld.add_action(move_to_pregrasp_node)
 
     return ld