Added a Joint Trajectory Action Server to enable position mode and Stretch Web Teleop support in Stretch Simulation

stretch_simulation/README.md

@@ -27,6 +27,7 @@ ros2 launch stretch_nav2 online_async_launch.py use_sim_time:=true
 ros2 launch stretch_nav2 navigation.launch.py use_slam:=true use_sim_time:=true use_rviz:=true teleop_type:=none
 
 # Terminal 2: Stretch Mujoco Driver
+export MUJOCO_GL=egl # On Ubuntu, tell Mujoco to use the GPU
 ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=true mode:=navigation
 
 # Terminal 3: Keyboard Teleop
@@ -67,7 +68,7 @@ ros2 param set /local_costmap/local_costmap  inflation_layer.inflation_radius 0.
 
 #### Pre-mapped scene
 
-There is a map included in the [ubuntu2204_ament_ws_files.zip](ubuntu2204_ament_ws_files.zip) file that you can use with navigation out of the box.
+There are maps included in the [ubuntu2204_ament_ws_files.zip](ubuntu2204_ament_ws_files.zip) file that you can use with navigation out of the box.
 
 If you set up your environment using the [Getting Started](#getting-started) section below, you should already have this map in your environment.
 
@@ -86,6 +87,34 @@ ros2 param set /global_costmap/global_costmap inflation_layer.inflation_radius 0
 ros2 param set /local_costmap/local_costmap  inflation_layer.inflation_radius 0.20
 ```
 
+### Web Teleop
+
+You can use Stretch Web Teleop with the Stretch Simulation environment! 
+
+
+Before you start, install the dependencies for Stretch Web Teleop by following these [instructions](#setting-up-stretch-web-teleop).
+
+
+Use the following commands to start Stretch Mujoco with Web Teleop:
+```shell
+parallel_terminal="gnome-terminal --tab -- /bin/bash -c " # or "xterm -e"
+
+# Terminal 1
+$parallel_terminal "MUJOCO_GL=egl ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true map:=${HELLO_FLEET_PATH}/maps/gshaped_modern1_robocasa.yaml" &
+
+# Terminal 2
+$parallel_terminal "ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py" &
+
+# Terminal 3
+$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; npm run localstorage" &
+
+# Terminal 4
+$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; sudo node ./server.js" &
+
+# Terminal 4
+$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; node start_robot_browser.js" &
+```
+
 ## Cameras and PointClouds
 
 Please use the `use_cameras:=true` argument to enable cameras and pointclouds. e.g. `ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=true mode:=navigation use_cameras:=true`
@@ -181,6 +210,12 @@ source /opt/ros/humble/setup.bash
 
 If you are not running this package on a robot NUC (which is _not_ [recommended](#system-requirements)), you will need to set up a ROS2 environment similar to the environment that ships with Stretch.
 
+First you should install `NodeJS>=21.x` and `npm` if you don't already have them:
+```shell
+curl -sL https://deb.nodesource.com/setup_22.x | sudo -E bash -
+sudo apt install -y nodejs
+```
+
 Please run these commands to install the environment. This will delete the existing `~/ament_ws` directory, so please proceed with caution.
 
 ```sh
@@ -193,7 +228,7 @@ unzip ubuntu2204_ament_ws_files.zip
 sudo cp -r ./ubuntu2204_ament_ws_files/etc/* /etc/
 cp -r ./ubuntu2204_ament_ws_files/stretch_user ~/
 
-bash ./ubuntu2204_ament_ws_files/env_install.sh
+bash ./ubuntu2204_ament_ws_files/stretch_create_ament_workspace.sh
 
 rm -rf ./ubuntu2204_ament_ws_files
 
@@ -206,6 +241,42 @@ colcon build
 source ./install/setup.bash
 ```
 
+### Setting up Stretch Web Teleop
+
+Make sure you've already completed everything under [Setting up `ament_ws`](#setting-up-ament_ws) above.
+
+Then run the following:
+
+```shell
+cd ~/ament_ws/src/stretch_web_teleop
+
+# Install both npm and pip dependencies:
+npm install --legacy-peer-deps
+pip install -r ./requirements.txt 
+
+# Install Playwright:
+npx install playwright
+sudo npx playwright install-deps 
+
+# Create a certificate for SSL/HTTPS:
+openssl req -new -x509 -nodes -out certificates/server.crt -keyout certificates/server.key
+
+touch .env
+echo certfile=server.crt >> .env
+echo keyfile=server.key >> .env
+```
+
+Some more steps to get IK for gripper working:
+```shell
+cd stretch_description/urdf
+cp ./stretch_uncalibrated.urdf stretch.urdf
+
+sudo apt install rpl
+./export_urdf.sh # It's okay if it fails on calibrated params
+
+mkdir -p $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf
+cp -r ./exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf
+```
 
 ### Setting up URDF
 
@@ -218,7 +289,7 @@ python3 -m pip install -U hello-robot-stretch-urdf
 
 git clone https://github.com/hello-robot/stretch_urdf.git --depth 1 /tmp/stretch_urdf
 
-python3 /tmp/stretch_urdf/tools/stretch_urdf_ros_update.py --model SE3 --tool eoa_wrist_dw3_tool_sg3
+python3 /tmp/stretch_urdf/tools/stretch_urdf_ros_update.py
 
 cd ~/ament_ws
 

stretch_simulation/launch/stretch_simulation_web_interface.launch.py

@@ -0,0 +1,248 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from ament_index_python.packages import get_package_share_path
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    GroupAction,
+    IncludeLaunchDescription,
+)
+from launch.conditions import LaunchConfigurationNotEquals
+from launch.launch_description_sources import (
+    FrontendLaunchDescriptionSource,
+    PythonLaunchDescriptionSource,
+)
+from launch.substitutions import (
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from stretch_mujoco_driver.stretch_mujoco_driver import DEFAULT_SIM_TOOL
+
+
+def generate_launch_description():
+    teleop_interface_package = str(get_package_share_path("stretch_web_teleop"))
+    rosbridge_package = str(get_package_share_path("rosbridge_server"))
+    stretch_navigation_path = str(get_package_share_directory("stretch_nav2"))
+
+    # Declare launch arguments
+    params_file = DeclareLaunchArgument(
+        "params",
+        default_value=[
+            PathJoinSubstitution(
+                [
+                    teleop_interface_package,
+                    "config",
+                    "configure_video_streams_params.yaml",
+                ]
+            )
+        ],
+    )
+    map = DeclareLaunchArgument(
+        "map", description="filepath to previously captured map", default_value=""
+    )
+    tts_engine = DeclareLaunchArgument(
+        "tts_engine",
+        description="name of the TTS engine. Either pyttsx3 or gtts.",
+        default_value="gtts",
+    )
+    certfile_arg = DeclareLaunchArgument("certfile", default_value="server.crt")
+    keyfile_arg = DeclareLaunchArgument("keyfile", default_value="server.key")
+    nav2_params_file_param = DeclareLaunchArgument(
+        "nav2_params_file",
+        default_value=os.path.join(
+            stretch_navigation_path, "config", "nav2_params.yaml"
+        ),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    # Start collecting nodes to launch
+    ld = LaunchDescription(
+        [
+            map,
+            tts_engine,
+            nav2_params_file_param,
+            params_file,
+            certfile_arg,
+            keyfile_arg,
+        ]
+    )
+
+    tf2_web_republisher_node = Node(
+        package="tf2_web_republisher_py",
+        executable="tf2_web_republisher",
+        name="tf2_web_republisher_node",
+    )
+    ld.add_action(tf2_web_republisher_node)
+
+    # Rosbridge Websocket
+    rosbridge_launch = IncludeLaunchDescription(
+        FrontendLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [rosbridge_package, "launch", "rosbridge_websocket_launch.xml"]
+            )
+        ),
+        launch_arguments={
+            "port": "9090",
+            "address": "localhost",
+            "ssl": "true",
+            "certfile": PathJoinSubstitution(
+                [
+                    teleop_interface_package,
+                    "certificates",
+                    LaunchConfiguration("certfile"),
+                ]
+            ),
+            "keyfile": PathJoinSubstitution(
+                [
+                    teleop_interface_package,
+                    "certificates",
+                    LaunchConfiguration("keyfile"),
+                ]
+            ),
+            "authenticate": "false",
+            "call_services_in_new_thread": "true",
+        }.items(),
+    )
+    ld.add_action(rosbridge_launch)
+
+    # Configure Video Streams
+    configure_video_streams_node = Node(
+        package="stretch_web_teleop",
+        executable="configure_video_streams.py",
+        name=f"configure_video_streams_gripper",
+        output="screen",
+        arguments=[
+            LaunchConfiguration("params"),
+            str(False),
+            "True",  # overhead"
+            "True",  # "realsense"
+            "True",  # "gripper"
+        ],
+        parameters=[
+            {
+                "has_beta_teleop_kit": False,
+                "stretch_tool": DEFAULT_SIM_TOOL,
+                "use_sim_time": True
+            }
+        ],
+        remappings=[("/gripper_camera/color/camera_info","/gripper_camera/camera_info")]
+    )
+    ld.add_action(configure_video_streams_node)
+
+    navigation_bringup_launch = GroupAction(
+        condition=LaunchConfigurationNotEquals("map", ""),
+        actions=[
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    [stretch_navigation_path, "/launch/bringup_launch.py"]
+                ),
+                launch_arguments={
+                    "use_sim_time": "true",
+                    "autostart": "true",
+                    "map": LaunchConfiguration("map"),
+                    "params_file": LaunchConfiguration("nav2_params_file"),
+                    "use_rviz": "false",
+                }.items(),
+            ),
+        ],
+    )
+    ld.add_action(navigation_bringup_launch)
+
+    ld.add_action(
+        ExecuteProcess(
+            cmd=[
+                [
+                    FindExecutable(name="ros2"),
+                    " service call ",
+                    "/reinitialize_global_localization ",
+                    "std_srvs/srv/Empty ",
+                    '"{}"',
+                ]
+            ],
+            shell=True,
+        ),
+    )
+
+    ld.add_action(
+        ExecuteProcess(
+            cmd=[
+                [
+                    FindExecutable(name="ros2"),
+                    " param set ",
+                    "/rosbridge_websocket ",
+                    "std_msgs/msg/Bool ",
+                    "true",
+                ]
+            ],
+            shell=True,
+        )
+    )
+
+    # ld.add_action(
+    #     ExecuteProcess(
+    #         cmd=[
+    #             [
+    #                 FindExecutable(name="ros2"),
+    #                 " param set ",
+    #                 "/gripper_camera ",
+    #                 "depth_module.enable_auto_exposure ",
+    #                 "true",
+    #             ]
+    #         ],
+    #         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=[{
+                "use_sim_time": True}],
+        remappings=[
+            ("/camera/aligned_depth_to_color/camera_info", "/camera/depth/camera_info"),
+            ("/camera/aligned_depth_to_color/image_raw/compressedDepth", "/camera/depth/image_rect_raw/compressed")
+        ]
+    )
+    ld.add_action(move_to_pregrasp_node)
+
+    # Text to speech
+    text_to_speech_node = Node(
+        package="stretch_web_teleop",
+        executable="text_to_speech.py",
+        output="screen",
+        arguments=[LaunchConfiguration("tts_engine")],
+        parameters=[],
+    )
+    ld.add_action(text_to_speech_node)
+
+    # if stretch_tool == "eoa_wrist_dw3_tool_tablet_12in":
+    #     detect_body_landmarks_node = Node(
+    #         package="stretch_show_tablet",
+    #         executable="detect_body_landmarks",
+    #         output="screen",
+    #     )
+    #     ld.add_action(detect_body_landmarks_node)
+
+    #     plan_tablet_pose_node = Node(
+    #         package="stretch_show_tablet",
+    #         executable="plan_tablet_pose_service",
+    #         output="screen",
+    #     )
+    #     ld.add_action(plan_tablet_pose_node)
+
+    #     show_tablet_node = Node(
+    #         package="stretch_show_tablet",
+    #         executable="show_tablet_server",
+    #         output="screen",
+    #     )
+    #     ld.add_action(show_tablet_node)
+
+    return ld