Click-to-Pregrasp

.gitignore

@@ -33,3 +33,9 @@ mkcert*
 dist/*
 
 outcomes/*
+
+# Compiled Python
+*/__pycache__
+
+# Specialized URDFs
+urdf/*.urdf

CMakeLists.txt

@@ -9,13 +9,27 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rosbridge_server REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 # find_package(tf2_web_republisher REQUIRED)
 
+##############################
+## Generate Custom Messages ##
+##############################
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/MoveToPregrasp.action"
+  DEPENDENCIES geometry_msgs
+)
+
+
 #############
 ## Install ##
 #############
 
+ament_python_install_package(${PROJECT_NAME}_helpers)
+
 # all install targets should use catkin DESTINATION variables
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
@@ -24,6 +38,7 @@ find_package(rosbridge_server REQUIRED)
 install(PROGRAMS
   nodes/compressed_image_visualizer.py
   nodes/configure_video_streams.py
+  nodes/move_to_pregrasp.py
   nodes/navigation_camera.py
   DESTINATION lib/${PROJECT_NAME}
 )
@@ -40,6 +55,10 @@ install(DIRECTORY nodes
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(DIRECTORY urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME}
 )

README.md

@@ -2,9 +2,9 @@
 
 This interface enables a user to remotely teleoperate a Stretch robot through a web browser. This website can be set up to teleoperate the robot remotely from anywhere in the world with an internet connection, or simply eyes-off teleop from the next room on a local network. The codebase is built on ROS2, WebRTC, Nav2, and TypeScript.
 
-# Setup
+# Setup & Installation
 
-The interface is compatible with the Stretch RE1, RE2 and SE3. It currently only supports Ubuntu 22.04 and ROS2 Humble. Upgrade your operating system if necessary ([instructions](<>)) and create a the Stretch ROS2 Humble workspace ([instructions](<>)). This will install all package dependencies and install the web teleop interface.
+The interface is compatible with the Stretch RE1, RE2 and SE3. It currently only supports Ubuntu 22.04 and ROS2 Humble. Upgrade your operating system if necessary ([instructions](https://docs.hello-robot.com/0.3/installation/robot_install/)) and create/update the Stretch ROS2 Humble workspace ([instructions](https://docs.hello-robot.com/0.3/installation/ros_workspace/)). This will install all package dependencies and install the web teleop interface.
 
 ## Installing Beta Teleop Cameras
 

action/MoveToPregrasp.action

@@ -0,0 +1,31 @@
+# MoveToPregrasp takes in the scaled (u,v) coordinates of a click on the Realsense camera,
+# as well as some configuration such as the direction of the pregrasp. It then moves
+# the robot to the pregrasp configuration, giving feedback on the way.
+
+# These should be in the range [0.0, 1.0]
+float64 scaled_u
+float64 scaled_v
+
+uint8 PREGRASP_DIRECTION_AUTO=0
+uint8 PREGRASP_DIRECTION_HORIZONTAL=1
+uint8 PREGRASP_DIRECTION_VERTICAL=2
+
+uint8 pregrasp_direction
+
+---
+# Result
+
+uint8 STATUS_SUCCESS=0
+uint8 STATUS_FAILURE=1
+uint8 STATUS_CANCELLED = 2
+uint8 STATUS_TIMEOUT = 3
+uint8 STATUS_GOAL_NOT_REACHABLE = 4
+uint8 STATUS_DEPROJECTION_FAILURE = 5
+uint8 STATUS_STRETCH_DRIVER_FAILURE = 6
+
+uint8 status
+---
+# Feedback
+float32 initial_distance_m
+float32 remaining_distance_m
+builtin_interfaces/Duration elapsed_time

launch/gripper_camera.launch.py

@@ -31,7 +31,12 @@ def generate_launch_description():
                     {"camera_name": "gripper_camera"},
                     {"io_method": "mmap"},
                 ],
-                remappings=[("/image_raw", "/gripper_camera/image_raw")],
+                remappings=[
+                    ("/image_raw", "/gripper_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", "/gripper_camera/image_raw/compressed")
+                ],
             ),
             # You can add more nodes or configurations here if needed.
         ]

launch/multi_camera.launch.py

@@ -12,6 +12,9 @@
     get_package_share_directory("stretch_core"), "config", "HighAccuracyPreset.json"
 )
 
+D435_RESOLUTION = "424x240x15"
+D405_RESOLUTION = "480x270x15"
+
 configurable_parameters = [
     {"name": "camera_namespace1", "default": "", "description": "namespace for camera"},
     {"name": "camera_name1", "default": "camera", "description": "camera unique name"},
@@ -23,18 +26,18 @@
     },
     {
         "name": "depth_module.depth_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "depth module profile",
     },
     {
         "name": "depth_module.infra_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "depth module infrared profile",
     },
     {"name": "enable_depth1", "default": "true", "description": "enable depth stream"},
     {
         "name": "rgb_camera.color_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "color image width",
     },
     {"name": "enable_color1", "default": "true", "description": "enable color stream"},
@@ -87,7 +90,7 @@
     },
     {
         "name": "depth_module.depth_profile2",
-        "default": "480x270x15",
+        "default": D405_RESOLUTION,
         "description": "depth module profile",
     },
     {
@@ -98,7 +101,7 @@
     {"name": "enable_depth2", "default": "true", "description": "enable depth stream"},
     {
         "name": "depth_module.color_profile2",
-        "default": "480x270x15",
+        "default": D405_RESOLUTION,
         "description": "color image width",
     },
     {"name": "enable_color2", "default": "true", "description": "enable color stream"},
@@ -164,6 +167,10 @@ def generate_launch_description():
                 src="/gripper_camera/color/image_rect_raw",
                 dst="/gripper_camera/image_raw",
             ),
+            SetRemap(
+                src="/gripper_camera/color/image_rect_raw/compressed",
+                dst="/gripper_camera/image_raw/compressed",
+            ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
                     [

launch/navigation_camera.launch.py

@@ -32,7 +32,12 @@ def generate_launch_description():
                     {"camera_name": "navigation_camera"},
                     {"io_method": "mmap"},
                 ],
-                remappings=[("/image_raw", "/navigation_camera/image_raw")],
+                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.
         ]

launch/web_interface.launch.py

@@ -505,4 +505,15 @@ def generate_launch_description():
     #         shell=True,
     #     )
     # )
+
+    # 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)
+
     return ld