diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index 394aadec..a3ff92bf 100644 --- a/stretch_simulation/README.md +++ b/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 diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py new file mode 100644 index 00000000..e374aea5 --- /dev/null +++ b/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 diff --git a/stretch_simulation/rviz/stretch_sim.rviz b/stretch_simulation/rviz/stretch_sim.rviz index 86405948..db5edefa 100644 --- a/stretch_simulation/rviz/stretch_sim.rviz +++ b/stretch_simulation/rviz/stretch_sim.rviz @@ -6,9 +6,12 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Camera1 - /Camera1/Topic1 + - /PointCloud21 + - /PointCloud21/Topic1 Splitter Ratio: 0.5 - Tree Height: 444 + Tree Height: 348 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -77,7 +80,7 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable + Reliability Policy: Best Effort Value: /scan_filtered Use Fixed Frame: true Use rainbow: true @@ -389,28 +392,6 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 20 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera/cam_d435i_rgb_raw - Value: true - Visibility: - Grid: true - LaserScan: true - Map: true - PointCloud2: false - RobotModel: true - TF: true - Value: false - Zoom Factor: 1 - Class: rviz_default_plugins/TF Enabled: false Frame Timeout: 15 @@ -446,6 +427,28 @@ Visualization Manager: Value: /map_updates Use Timestamp: false Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /camera/color/image_raw + Value: true + Visibility: + Grid: true + LaserScan: true + Map: true + PointCloud2: true + RobotModel: true + TF: true + Value: false + Zoom Factor: 1 - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -475,8 +478,8 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /pointcloud/cam_d435i_depth + Reliability Policy: Best Effort + Value: /camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -526,7 +529,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.2111217975616455 + Distance: 4.92409086227417 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -541,20 +544,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4653986096382141 + Pitch: 1.0253984928131104 Target Frame: Value: Orbit (rviz) - Yaw: 5.638580799102783 + Yaw: 6.02358341217041 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f700000359fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000023f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000282000001140000002800ffffff000000010000015f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000359000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000041e0000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001df000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000222000001590000002800fffffffb0000000c00430061006d00650072006101000001b5000001c60000000000000000000000010000015f0000033efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000005afc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003d80000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -563,6 +566,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 1920 - Y: 0 + Width: 1850 + X: 70 + Y: 27 diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index bc3e98f4..628684fc 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 +from functools import cache import time import copy import pickle @@ -17,7 +18,11 @@ from rclpy.duration import Duration from control_msgs.action import FollowJointTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint, MultiDOFJointTrajectory, JointTrajectory +from trajectory_msgs.msg import ( + JointTrajectoryPoint, + MultiDOFJointTrajectory, + JointTrajectory, +) import hello_helpers.hello_misc as hm @@ -26,20 +31,163 @@ if TYPE_CHECKING: from stretch_mujoco_driver.stretch_mujoco_driver import StretchMujocoDriver +from stretch_mujoco.enums.actuators import Actuators + +import rclpy +import rclpy.action +from rclpy.node import Node +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint +from rclpy.action import ActionServer +from rclpy.executors import MultiThreadedExecutor + + class JointTrajectoryAction: - def __init__(self, node: "StretchMujocoDriver", action_server_rate_hz:int): + def __init__(self, node: "StretchMujocoDriver", action_server_rate_hz: int): self.node = node self._goal_handle = None - self._goal_lock = threading.Lock() - self.server = ActionServer(self.node, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory', - execute_callback=self.execute_cb, - cancel_callback=self.cancel_cb, - goal_callback=self.goal_cb, - handle_accepted_callback=self.handle_accepted_cb, - callback_group=node.main_group) - - def goal_cb(self, goal_request: FollowJointTrajectory.Goal): raise NotImplementedError() - def handle_accepted_cb(self, goal_handle:ServerGoalHandle):raise NotImplementedError() - def execute_cb(self, goal_handle:ServerGoalHandle): raise NotImplementedError() - def cancel_cb(self, goal_handle:ServerGoalHandle):raise NotImplementedError() \ No newline at end of file + self._action_server = ActionServer( + self.node, + FollowJointTrajectory, + "/stretch_controller/follow_joint_trajectory", + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + handle_accepted_callback=self.handle_accepted_callback, + callback_group=node.main_group, + ) + self.timeout = 0.2 # seconds + self.last_goal_time = self.node.get_clock().now().to_msg() + + self.latest_goal_id = 0 + + def handle_accepted_callback(self, goal_handle: ServerGoalHandle): + # This server only allows one goal at a time + if self._goal_handle is not None and self._goal_handle.is_active: + self.node.get_logger().info("Aborting previous goal") + # Abort the existing goal + # self._goal_handle.abort() \TODO(@hello-atharva): This is causing state transition issues. + self._goal_handle = goal_handle + + # Increment goal ID + self.latest_goal_id += 1 + + # Launch an asynch coroutine to execute the goal + goal_handle.execute() + + def goal_callback(self, goal_request): + self.node.get_logger().info(f"Received goal request, {goal_request}") + new_goal_time = self.node.get_clock().now().to_msg() + time_duration = (new_goal_time.sec + new_goal_time.nanosec * pow(10, -9)) - ( + self.last_goal_time.sec + self.last_goal_time.nanosec * pow(10, -9) + ) + + if ( + self._goal_handle is not None + and self._goal_handle.is_active + and (time_duration < self.timeout) + ): + return ( + GoalResponse.REJECT + ) # Reject goal if another goal is currently active + + self.last_goal_time = self.node.get_clock().now().to_msg() + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle): + self.node.get_logger().info("Received cancel request") + return CancelResponse.ACCEPT + + def execute_callback(self, goal_handle): + self.node.get_logger().info("Executing trajectory...") + + trajectory = goal_handle.request.trajectory + joint_names = trajectory.joint_names + last_positions = {name: 0.0 for name in joint_names} + + for point in trajectory.points: + positions: list[float] = point.positions + velocities: list[float | None] = ( + point.velocities if point.velocities else [None] * len(joint_names) + ) + + actuators_in_use = [] + + for i, joint in enumerate(joint_names): + try: + actuator = get_actuator_by_joint_names_in_command_groups(joint) + except: + self.node.get_logger().error(f"No command group for joint '{joint}'") + continue + + target_position = positions[i] + delta = target_position - last_positions[joint] + last_positions[joint] = target_position + velocity = velocities[i] + + if (actuator == Actuators.left_wheel_vel or actuator == Actuators.right_wheel_vel) and velocity is not None: + self.node.sim.set_base_velocity(velocity, 0) + continue + + self.node.sim.move_to(actuator, target_position) + + actuators_in_use.append(actuator) + + for actuator in actuators_in_use: + self.node.sim.wait_until_at_setpoint(actuator) + + for actuator in [Actuators.left_wheel_vel, Actuators.right_wheel_vel]: + self.node.sim.wait_while_is_moving(actuator) + + # Simulate wait until point.time_from_start + # self._wait_until( + # point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 + # ) + + goal_handle.succeed() + result = FollowJointTrajectory.Result() + self.node.get_logger().info("Trajectory execution complete") + return result + + def _wait_until(self, seconds): + loop_rate = self.node.create_rate(10) + t_start = self.node.get_clock().now().seconds_nanoseconds()[0] + while ( + self.node.get_clock().now().seconds_nanoseconds()[0] - t_start + ) < seconds: + loop_rate.sleep() + + +@cache +def get_actuator_by_joint_names_in_command_groups(joint_name: str) -> Actuators: + """ + Joint names defined by stretch_core command groups, return their Actuator here. + """ + if joint_name == "joint_left_wheel": + return Actuators.left_wheel_vel + if joint_name == "joint_right_wheel": + return Actuators.right_wheel_vel + if joint_name == 'translate_mobile_base' or joint_name == 'position': + return Actuators.base_translate + if joint_name == 'rotate_mobile_base': + return Actuators.base_rotate + + if joint_name == "joint_lift": + return Actuators.lift + if joint_name == "joint_arm" or joint_name == "wrist_extension": + return Actuators.arm + if joint_name == "joint_wrist_yaw": + return Actuators.wrist_yaw + if joint_name == "joint_wrist_pitch": + return Actuators.wrist_pitch + if joint_name == "joint_wrist_roll": + return Actuators.wrist_roll + if joint_name == "joint_gripper_slide" or joint_name == "joint_gripper_finger_left" or joint_name == "joint_gripper_finger_right" or joint_name == "gripper_aperture": + return Actuators.gripper + if joint_name == "joint_head_pan": + return Actuators.head_pan + if joint_name == "joint_head_tilt": + return Actuators.head_tilt + + raise NotImplementedError(f"Actuator for {joint_name} is not defined.") diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index b2d02d36..33d532d6 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -1,13 +1,17 @@ #! /usr/bin/env python3 +import array import copy +from functools import cache +import cv2 import numpy as np import threading +from sensor_msgs.msg._compressed_image import CompressedImage from stretch_mujoco import StretchMujocoSimulator from stretch_mujoco.enums.actuators import Actuators from stretch_mujoco.enums.stretch_sensors import StretchSensors -from stretch_mujoco.enums.stretch_cameras import StretchCameras +from stretch_mujoco.enums.stretch_cameras import CameraSettings, StretchCameras from stretch_mujoco.robocasa_gen import ( layout_from_str, style_from_str, @@ -16,6 +20,7 @@ layouts, ) +from rclpy.qos import QoSProfile, ReliabilityPolicy from stretch_core.rwlock import RWLock from stretch_mujoco_driver.joint_trajectory_server import JointTrajectoryAction import tf2_ros @@ -52,8 +57,9 @@ from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField, Joy from std_msgs.msg import Bool, String, Float64MultiArray +from hello_helpers.joint_qpos_conversion import SE3_dw3_sg3_Idx +from hello_helpers.joint_qpos_conversion import get_Idx from hello_helpers.gripper_conversion import GripperConversion -from hello_helpers.joint_qpos_conversion import get_Idx, UnsupportedToolError from hello_helpers.gamepad_conversion import ( unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, @@ -68,6 +74,14 @@ from rclpy import time as rclpyTime +DEFAULT_TIMEOUT = 0.5 +DEFAULT_GOAL_TIMEOUT = 10.0 +DEFAULT_ROBOCASA_TASK = "PnPCounterToCab" +DEFAULT_ACTION_SERVER_HZ = 30.0 +DEFAULT_JOINT_STATE_HZ = 30.0 +DEFAULT_SIM_TOOL = "eoa_wrist_dw3_tool_sg3" + + class StretchMujocoDriver(Node): def __init__(self): @@ -76,7 +90,7 @@ def __init__(self): self.declare_parameter("use_cameras", False) self.declare_parameter("use_mujoco_viewer", True) self.declare_parameter("use_robocasa", True) - self.declare_parameter("robocasa_task", "PnPCounterToCab") + self.declare_parameter("robocasa_task", DEFAULT_ROBOCASA_TASK) self.declare_parameter("robocasa_layout", None) self.declare_parameter("robocasa_style", None) @@ -87,7 +101,7 @@ def __init__(self): use_robocasa = self.get_parameter("use_robocasa").value if use_robocasa: - robocasa_task = self.get_parameter("robocasa_task").value + robocasa_task: str | None = self.get_parameter("robocasa_task").value robocasa_layout = self.get_parameter("robocasa_layout").value robocasa_style = self.get_parameter("robocasa_style").value @@ -114,13 +128,14 @@ def __init__(self): robocasa_style = -1 model, xml, objects_info = model_generation_wizard( - task=robocasa_task, + task=robocasa_task or DEFAULT_ROBOCASA_TASK, layout=robocasa_layout, style=robocasa_style, ) sim = StretchMujocoSimulator( model=model, + camera_hz=10, cameras_to_use=( StretchCameras.all() if use_cameras else StretchCameras.none() ), @@ -224,25 +239,20 @@ def set_robot_streaming_position_callback(self, msg): def move_to_position(self, qpos): try: - try: - Idx = get_Idx("tool_stretch_gripper") - except UnsupportedToolError: - self.get_logger().error( - "Unsupported tool for streaming position control." - ) - return + Idx: SE3_dw3_sg3_Idx = get_Idx(DEFAULT_SIM_TOOL) # type: ignore + if len(qpos) != Idx.num_joints: self.get_logger().error( "Received qpos does not match the number of joints in the robot" ) return - self.sim.move_to("arm", qpos[Idx.ARM]) - self.sim.move_to("lift", qpos[Idx.LIFT]) - self.sim.move_to("wrist_yaw", qpos[Idx.WRIST_YAW]) - self.sim.move_to("wrist_pitch", qpos[Idx.WRIST_PITCH]) - self.sim.move_to("wrist_roll", qpos[Idx.WRIST_ROLL]) - self.sim.move_to("head_pan", qpos[Idx.HEAD_PAN]) - self.sim.move_to("head_tilt", qpos[Idx.HEAD_TILT]) + self.sim.move_to(Actuators.arm, qpos[Idx.ARM]) + self.sim.move_to(Actuators.lift, qpos[Idx.LIFT]) + self.sim.move_to(Actuators.wrist_yaw, qpos[Idx.WRIST_YAW]) + self.sim.move_to(Actuators.wrist_pitch, qpos[Idx.WRIST_PITCH]) + self.sim.move_to(Actuators.wrist_roll, qpos[Idx.WRIST_ROLL]) + self.sim.move_to(Actuators.head_pan, qpos[Idx.HEAD_PAN]) + self.sim.move_to(Actuators.head_tilt, qpos[Idx.HEAD_TILT]) if ( abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and abs(qpos[Idx.BASE_ROTATE]) > 0.0 @@ -259,6 +269,32 @@ def move_to_position(self, qpos): pos = self.gripper_conversion.finger_to_robotis(qpos[Idx.GRIPPER]) self.sim.move_to(Actuators.gripper, pos) + for actuator in [ + Actuators.arm, + Actuators.lift, + Actuators.wrist_pitch, + Actuators.wrist_roll, + Actuators.wrist_yaw, + Actuators.head_pan, + Actuators.head_tilt, + Actuators.gripper, + ]: + succeeded = self.sim.wait_until_at_setpoint(actuator) + if not succeeded: + raise Exception( + f"{actuator} failed to move to {self.sim.data_proxies.get_command().move_to[actuator.name]}" + ) + + for actuator in [ + Actuators.base_translate, + Actuators.base_rotate, + ]: + succeeded = self.sim.wait_while_is_moving(actuator) + if not succeeded: + raise Exception( + f"{actuator} failed to move to {self.sim.data_proxies.get_command().move_to[actuator.name]}" + ) + self.get_logger().info(f"Moved to position qpos: {qpos}") except Exception as e: self.get_logger().error("Failed to move to position: {0}".format(e)) @@ -286,7 +322,7 @@ def command_mobile_base_velocity_and_publish_state(self): self.sim.set_base_velocity( self.linear_velocity_mps, self.angular_velocity_radps ) - elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): + elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): # type: ignore # self.sim.set_base_velocity(0.0, 0.0) self.sim.move_by(Actuators.base_translate, 0.0) else: @@ -334,9 +370,11 @@ def command_mobile_base_velocity_and_publish_state(self): t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] self.tf_broadcaster.sendTransform(t) - + # This is important, otherwise all the joints are not transformed correctly. The alternative is to broadcast a static_transform, but that doesn't help if another node is trying to lookup transforms. - self.tf_buffer.wait_for_transform_async("base_link", "link_lift", rclpyTime.Time(seconds=0)) + self.tf_buffer.wait_for_transform_async( + "base_link", "link_lift", rclpyTime.Time(seconds=0) + ) b = TransformStamped() b.header.stamp = current_time @@ -433,12 +471,12 @@ def command_mobile_base_velocity_and_publish_state(self): ################################################## # publish homed status homed_status = Bool() - # homed_status.data = bool(self.sim.is_homed()) + homed_status.data = True self.homed_pub.publish(homed_status) # publish runstop event runstop_event = Bool() - # runstop_event.data = robot_status.pimu.runstop_event + runstop_event.data = self.is_runstopped() self.runstop_event_pub.publish(runstop_event) # publish stretch_driver operation mode @@ -448,7 +486,7 @@ def command_mobile_base_velocity_and_publish_state(self): # publish end of arm tool tool_msg = String() - # tool_msg.data = self.sim.end_of_arm.name + tool_msg.data = "eoa_wrist_dw3_tool_sg3" self.tool_pub.publish(tool_msg) # publish streaming position status @@ -665,7 +703,9 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ... # Lidar is disabled, get_data() throws a ValueError camera_data = self.sim.pull_camera_data() - for camera, frame in camera_data.get_all(auto_rotate=True).items(): + for camera, frame in camera_data.get_all( + auto_rotate=False, auto_correct_rgb=True + ).items(): header = Header() header.frame_id = get_camera_frame(camera) header.stamp = current_time @@ -677,15 +717,22 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ) self.camera_publishers[camera.name].publish(ros_image) - settings = camera.initial_camera_settings + settings: CameraSettings = camera.initial_camera_settings camera_info = create_camera_info( - fovy=settings.field_of_view_vertical_in_degrees, - width=settings.width, - height=settings.height, + camera_settings=settings, frame_id=header.frame_id, timestamp=current_time, ) - self.camera_info_pub.publish(camera_info) + self.camera_info_publishers[camera.name].publish(camera_info) + + if camera.is_depth: + ros_image_compressed = compress_depth_image(frame) + else: + ros_image_compressed: CompressedImage = ( + self.bridge.cv2_to_compressed_imgmsg(frame, "png") + ) + ros_image_compressed.header.frame_id = get_camera_frame(camera) + self.camera_compressed_publishers[camera.name].publish(ros_image_compressed) if camera.is_depth: if camera == StretchCameras.cam_d405_depth: @@ -740,10 +787,6 @@ def turn_on_position_mode(self): # mobile base. It does not update the virtual prismatic # joint. The frames associated with 'floor_link' and # 'base_link' become identical in this mode. - raise NotImplementedError( - "Position Mode is not yet supported in StretchMujocoDriver." - ) - def code_to_run(): # self.sim.base.enable_pos_incr_mode() ... @@ -888,39 +931,33 @@ def deactivate_streaming_position_service_callback(self, request, response): def get_joint_states_callback(self, request, response): joint_limits = JointState() joint_limits.header.stamp = self.get_clock().now().to_msg() - cgs = list( - set(self.joint_trajectory_action.command_groups) - - set( - [ - self.joint_trajectory_action.mobile_base_cg, - self.joint_trajectory_action.gripper_cg, - ] - ) - ) - for cg in cgs: - lower_limit, upper_limit = cg.range - joint_limits.name.append(cg.name) - joint_limits.position.append( - lower_limit - ) # Misuse position array to mean lower limits - joint_limits.velocity.append( - upper_limit - ) # Misuse velocity array to mean upper limits - - gripper_cg = self.joint_trajectory_action.gripper_cg - if gripper_cg is not None: - lower_aperture_limit, upper_aperture_limit = gripper_cg.range_aperture_m - joint_limits.name.append("gripper_aperture") - joint_limits.position.append(lower_aperture_limit) - joint_limits.velocity.append(upper_aperture_limit) - - lower_finger_limit, upper_finger_limit = gripper_cg.range_finger_rad - joint_limits.name.append("joint_gripper_finger_left") - joint_limits.position.append(lower_finger_limit) - joint_limits.velocity.append(upper_finger_limit) - joint_limits.name.append("joint_gripper_finger_right") - joint_limits.position.append(lower_finger_limit) - joint_limits.velocity.append(upper_finger_limit) + + joint_limits_from_sim = self.sim.pull_joint_limits() + for actuator, min_max in joint_limits_from_sim.items(): + joint_name = actuator.get_joint_names_in_mjcf()[0] + min_limit, max_limit = min_max + if actuator == Actuators.arm: + joint_name = "joint_arm" # Instead of the telescoping names + max_limit *= 4 # 4x the telescoping limit + if actuator == Actuators.gripper: + joint_name = "gripper_aperture" # A different mapping from stretch_core command_groups + if actuator in [ + Actuators.gripper_left_finger, + Actuators.gripper_right_finger, + ]: + joint_name = joint_name.replace( + "_open", "" + ) # A different mapping from stretch_core command_groups + + joint_limits.name.append(joint_name) # type:ignore + joint_limits.position.append(min_limit) + joint_limits.velocity.append(max_limit) + + # add "wrist_extension" because it's expected downstream + arm_joint_limit = joint_limits_from_sim[Actuators.arm] + joint_limits.name.append("wrist_extension") # type:ignore + joint_limits.position.append(arm_joint_limit[0]) + joint_limits.velocity.append(arm_joint_limit[1] * 4) # 4x the telescoping limit self.joint_limits_pub.publish(joint_limits) response.success = True @@ -928,16 +965,17 @@ def get_joint_states_callback(self, request, response): return response def self_collision_avoidance_callback(self, request, response): - enable_self_collision_avoidance = request.data - if enable_self_collision_avoidance: - self.sim.enable_collision_mgmt() - else: - self.sim.disable_collision_mgmt() + # enable_self_collision_avoidance = request.data + # if enable_self_collision_avoidance: + # self.sim.enable_collision_mgmt() + # else: + # self.sim.disable_collision_mgmt() - response.success = True - response.message = ( - f"is self collision avoidance enabled: {enable_self_collision_avoidance}" - ) + response.success = False + response.message = "collision avoidance is not supported in simulation mode." + # response.message = ( + # f"is self collision avoidance enabled: {enable_self_collision_avoidance}" + # ) return response def parameter_callback(self, parameters: list[Parameter]) -> SetParametersResult: @@ -946,9 +984,9 @@ def parameter_callback(self, parameters: list[Parameter]) -> SetParametersResult """ for parameter in parameters: if parameter.name == "default_goal_timeout_s": - self.default_goal_timeout_s = parameter.value + self.default_goal_timeout_s = parameter.value or DEFAULT_GOAL_TIMEOUT self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s + seconds=self.default_goal_timeout_s # type: ignore ) self.get_logger().info( f"Set default_goal_timeout_s to {self.default_goal_timeout_s}" @@ -983,6 +1021,9 @@ def stow_the_robot(self): self.change_mode(last_robot_mode, lambda: None) return True, "Stowed." + def is_runstopped(self): + return self.robot_mode == "runstopped" + def runstop_the_robot(self, runstopped, just_change_mode=False): if runstopped: self.robot_mode_rwlock.acquire_read() @@ -993,8 +1034,6 @@ def runstop_the_robot(self, runstopped, just_change_mode=False): if already_runstopped: return self.change_mode("runstopped", lambda: None) - if not just_change_mode: - self.sim.pimu.runstop_event_trigger() else: self.robot_mode_rwlock.acquire_read() already_not_runstopped = self.robot_mode != "runstopped" @@ -1002,8 +1041,6 @@ def runstop_the_robot(self, runstopped, just_change_mode=False): if already_not_runstopped: return self.change_mode(self.prerunstop_mode, lambda: None) - if not just_change_mode: - self.sim.pimu.runstop_event_reset() # ROS Setup ################# def ros_setup(self): @@ -1044,17 +1081,14 @@ def ros_setup(self): self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.tf_static_broadcaster = StaticTransformBroadcaster(self) - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) - stretch_core_path = get_package_share_path("stretch_core") self.declare_parameter( "controller_calibration_file", str(stretch_core_path / "config" / "controller_calibration_head.yaml"), ) - # large_ang = np.radians(45.0) # filename = self.get_parameter('controller_calibration_file').value # self.get_logger().debug('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename)) @@ -1113,25 +1147,52 @@ def ros_setup(self): self.odom_pub = self.create_publisher(Odometry, "odom", 1) self.laser_scan_pub = self.create_publisher( - LaserScan, "/scan_filtered", qos_profile=5 + LaserScan, + "/scan_filtered", + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT), ) self.camera_publishers = { camera.name: self.create_publisher( - Image, f"/camera/{camera.name}_raw", qos_profile=5 + Image, + get_camera_topic_name(camera), + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), + ) + for camera in self.sim._cameras_to_use + } + self.camera_compressed_publishers = { + camera.name: self.create_publisher( + CompressedImage, + f"{get_camera_topic_name(camera)}/compressed", + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), ) for camera in self.sim._cameras_to_use } self.pointcloud_publishers = { camera.name: self.create_publisher( - PointCloud2, f"/pointcloud/{camera.name}", qos_profile=5 + PointCloud2, + get_camera_pointcloud_topic_name(camera), + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), ) for camera in self.sim._cameras_to_use if camera.is_depth } - self.camera_info_pub = self.create_publisher( - CameraInfo, f"/camera/camera_info", qos_profile=5 - ) + self.camera_info_publishers = { + camera.name: self.create_publisher( + CameraInfo, + get_camera_info_topic_name(camera), + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), + ) + for camera in self.sim._cameras_to_use + } self.clock_pub = self.create_publisher( msg_type=Clock, topic="/clock", qos_profile=5 @@ -1183,29 +1244,34 @@ def ros_setup(self): callback_group=self.main_group, ) - self.declare_parameter("rate", 30.0) - self.joint_state_rate = self.get_parameter("rate").value + self.declare_parameter("rate", DEFAULT_JOINT_STATE_HZ) + self.joint_state_rate: float = ( + self.get_parameter("rate").value or DEFAULT_JOINT_STATE_HZ + ) + self.declare_parameter( "timeout", - 0.5, + DEFAULT_TIMEOUT, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description="Timeout (sec) after which Twist/Joy commands are considered stale", ), ) - self.timeout_s = self.get_parameter("timeout").value - self.timeout = Duration(seconds=self.timeout_s) + self.timeout_s = self.get_parameter("timeout").value or DEFAULT_TIMEOUT + self.timeout = Duration(seconds=self.timeout_s) # type: ignore self.declare_parameter( "default_goal_timeout_s", - 10.0, + DEFAULT_GOAL_TIMEOUT, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description="Default timeout (sec) for goal execution", ), ) - self.default_goal_timeout_s = self.get_parameter("default_goal_timeout_s").value + self.default_goal_timeout_s: float = ( + self.get_parameter("default_goal_timeout_s").value or DEFAULT_GOAL_TIMEOUT + ) self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s + seconds=self.default_goal_timeout_s # type: ignore ) self.get_logger().info(f"rate = {self.joint_state_rate} Hz") self.get_logger().info(f"twist timeout = {self.timeout_s} s") @@ -1312,21 +1378,23 @@ def ros_setup(self): # start action server for joint trajectories self.declare_parameter("fail_out_of_range_goal", False) - self.fail_out_of_range_goal: bool = self.get_parameter( - "fail_out_of_range_goal" - ).value + self.fail_out_of_range_goal = bool( + self.get_parameter("fail_out_of_range_goal").value + ) self.declare_parameter( "fail_if_motor_initial_point_is_not_trajectory_first_point", True ) - self.fail_if_motor_initial_point_is_not_trajectory_first_point: bool = ( + self.fail_if_motor_initial_point_is_not_trajectory_first_point: bool = bool( self.get_parameter( "fail_if_motor_initial_point_is_not_trajectory_first_point" ).value ) - self.declare_parameter("action_server_rate", 30.0) - self.action_server_rate: float = self.get_parameter("action_server_rate").value + self.declare_parameter("action_server_rate", DEFAULT_ACTION_SERVER_HZ) + self.action_server_rate: float = ( + self.get_parameter("action_server_rate").value or DEFAULT_ACTION_SERVER_HZ + ) self.joint_trajectory_action = JointTrajectoryAction( self, self.action_server_rate @@ -1345,7 +1413,7 @@ def ros_setup(self): # start loop to command the mobile base velocity, publish # odometry, and publish joint states - timer_period = 1.0 / self.joint_state_rate + timer_period: float = 1.0 / self.joint_state_rate self.timer = self.create_timer( timer_period, self.command_mobile_base_velocity_and_publish_state, @@ -1439,49 +1507,112 @@ def create_pointcloud_rgb_msg( return cloud_msg +__COMPRESSED_DEPTH_16UC1_HEADER = array.array("B", [0] * 12) + + +def compress_depth_image(frame: np.ndarray): + """ + Converts a F32 depth map in meters to a U16 map in millimeters + """ + normalized_array = (frame * 1000).astype(np.uint16) + + _, encoded_image = cv2.imencode(".png", normalized_array) + + ros_image_compressed = CompressedImage() + ros_image_compressed.format = "16uc1; compressedDepth" + ros_image_compressed.data = __COMPRESSED_DEPTH_16UC1_HEADER + array.array( + "B", encoded_image.tobytes() + ) + + return ros_image_compressed + + def create_camera_info( - fovy: float, width: int, height: int, frame_id: str, timestamp: TimeMsg + camera_settings: CameraSettings, frame_id: str, timestamp: TimeMsg ): camera_info_msg = CameraInfo() camera_info_msg.header = Header() camera_info_msg.header.stamp = timestamp camera_info_msg.header.frame_id = frame_id - camera_info_msg.width = width - camera_info_msg.height = height + camera_info_msg.width = camera_settings.width + camera_info_msg.height = camera_settings.height camera_info_msg.distortion_model = "plumb_bob" - focal_scaling = (0.5 * height) / np.tan((fovy * np.pi / 360.0)) - camera_info_msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] - camera_info_msg.k = [ - focal_scaling, - 0.0, - width / 2, - 0.0, - focal_scaling, - height / 2, - 0.0, - 0.0, - 1.0, - ] - camera_info_msg.p = [ - focal_scaling, - 0.0, - width / 2, - 0.0, - 0.0, - focal_scaling, - height / 2, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - ] + camera_info_msg.d = camera_settings.get_distortion_params_d() + camera_info_msg.k = camera_settings.get_intrinsic_params_k() + camera_info_msg.p = camera_settings.get_projection_matrix_p() + + if camera_settings.crop is not None: + camera_info_msg.roi.x_offset = camera_settings.crop.x_offset + camera_info_msg.roi.y_offset = camera_settings.crop.y_offset + camera_info_msg.roi.width = camera_settings.crop.width + camera_info_msg.roi.height = camera_settings.crop.height return camera_info_msg +@cache +def get_camera_topic_name(camera: StretchCameras): + """ + Topic names to match the camera topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + return "/gripper_camera/image_raw" + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/image_rect_raw" + if camera == StretchCameras.cam_d435i_rgb: + return "/camera/color/image_raw" + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/image_rect_raw" + if camera == StretchCameras.cam_nav_rgb: + return "/navigation_camera/image_raw" + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + + +@cache +def get_camera_info_topic_name(camera: StretchCameras): + """ + Topic names to match the camera_info topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + return "/gripper_camera/camera_info" + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/camera_info" + if camera == StretchCameras.cam_d435i_rgb: + return "/camera/color/camera_info" + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/camera_info" + if camera == StretchCameras.cam_nav_rgb: + return "/navigation_camera/camera_info" + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + + +@cache +def get_camera_pointcloud_topic_name(camera: StretchCameras): + """ + Topic names to match the pointcloud2 topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/color/points" + if camera == StretchCameras.cam_d435i_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/color/points" + if camera == StretchCameras.cam_nav_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + + +@cache def get_camera_frame(camera: StretchCameras): + """ + Matches the simulation camera with the optical frame on the robot urdf. + """ if camera == StretchCameras.cam_d405_rgb: return "gripper_camera_color_optical_frame" if camera == StretchCameras.cam_d405_depth: @@ -1496,35 +1627,6 @@ def get_camera_frame(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} frame is not implemented") -def get_joint_names_in_mjcf(actuator): - # Temporary until stretch_mujoco PR#39 is merged in, then we can use Actuators.get_joint_names_in_mjcf: - """ - An actuator may have multiple joints. Return their names here. - """ - if actuator == Actuators.left_wheel_vel: - return ["joint_left_wheel"] - if actuator == Actuators.right_wheel_vel: - return ["joint_right_wheel"] - if actuator == Actuators.lift: - return ["joint_lift"] - if actuator == Actuators.arm: - return ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3"] - if actuator == Actuators.wrist_yaw: - return ["joint_wrist_yaw"] - if actuator == Actuators.wrist_pitch: - return ["joint_wrist_pitch"] - if actuator == Actuators.wrist_roll: - return ["joint_wrist_roll"] - if actuator == Actuators.gripper: - return ["joint_gripper_slide"] - if actuator == Actuators.head_pan: - return ["joint_head_pan"] - if actuator == Actuators.head_tilt: - return ["joint_head_tilt"] - - raise NotImplementedError(f"Joint names for {actuator} are not defined.") - - def main(): rclpy.init() diff --git a/stretch_simulation/ubuntu2204_ament_ws_files.zip b/stretch_simulation/ubuntu2204_ament_ws_files.zip index f5bdc012..2cc490c3 100644 Binary files a/stretch_simulation/ubuntu2204_ament_ws_files.zip and b/stretch_simulation/ubuntu2204_ament_ws_files.zip differ