diff --git a/.gitmodules b/.gitmodules index 38091a9a1..18638b83b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -33,3 +33,11 @@ [submodule "src/moveit_pro_sam2"] path = src/moveit_pro_sam2 url = https://github.com/PickNikRobotics/moveit_pro_sam2.git +[submodule "src/external_dependencies/moveit_pro_experimental_behaviors"] + path = src/external_dependencies/moveit_pro_experimental_behaviors + url = git@github.com:PickNikRobotics/moveit_pro_experimental_behaviors.git + branch = feat/pose-vector-basis-snapshot-behaviors +[submodule "src/external_dependencies/ROS-TCP-Endpoint"] + path = src/external_dependencies/ROS-TCP-Endpoint + url = https://github.com/Unity-Technologies/ROS-TCP-Endpoint.git + branch = main-ros2 diff --git a/src/external_dependencies/ROS-TCP-Endpoint b/src/external_dependencies/ROS-TCP-Endpoint new file mode 160000 index 000000000..54c1a64b6 --- /dev/null +++ b/src/external_dependencies/ROS-TCP-Endpoint @@ -0,0 +1 @@ +Subproject commit 54c1a64b6d5ef6ffa0a0431570bb74329b79b15b diff --git a/src/external_dependencies/moveit_pro_experimental_behaviors b/src/external_dependencies/moveit_pro_experimental_behaviors new file mode 160000 index 000000000..c58ea6cd6 --- /dev/null +++ b/src/external_dependencies/moveit_pro_experimental_behaviors @@ -0,0 +1 @@ +Subproject commit c58ea6cd605be6024c6d739ced76dd4917c9d12d diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index e4873abf5..8798b6953 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -20,6 +20,14 @@ hardware: - mujoco_model: "description/scene.xml" - mujoco_viewer: false + # Override the inherited (empty) drivers-to-persist launch so the Meta Quest + # ros_tcp_endpoint node starts with the sim and persists across agent_bridge + # restarts. Without it, /right_controller_odom and the button-event topics + # never publish, and the "Quest Teleop" objective blocks forever. + simulated_robot_driver_persist_launch_file: + package: "lab_sim" + path: "launch/sim/robot_drivers_to_persist_sim.launch.py" + moveit_params: joint_limits: package: "lab_sim" @@ -51,6 +59,8 @@ objectives: - "moveit_pro::behaviors::MujocoBehaviorsLoader" lab_sim: - "lab_sim_behaviors::LabSimBehaviorsLoader" + experimental: + - "experimental_behaviors::ExperimentalBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: diff --git a/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py new file mode 100644 index 000000000..7258d2967 --- /dev/null +++ b/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -0,0 +1,49 @@ +# Persistent driver-side processes for lab_sim. +# +# Launched by MoveIt Pro via the simulated_robot_driver_persist_launch_file +# config field. Lives in the "drivers to persist" lifecycle so it survives +# agent_bridge restarts — that matters because the Meta Quest headset holds +# a TCP socket open against ros_tcp_endpoint, and tearing that down would +# force the user to reconnect from the headset. + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + ros_tcp_endpoint_node = Node( + package="ros_tcp_endpoint", + executable="default_server_endpoint", + name="ros_tcp_endpoint", + emulate_tty=True, + parameters=[ + {"ROS_IP": "192.168.1.34"}, + ], + output="screen", + ) + + # Debug-only static TF linking the disjoint quest TF root into the robot tree + # at the world frame. The numerical values are wrong — quest and world have + # no real geometric relationship — but the link satisfies the canTransform() + # check inside marker_utils::transformPoseToBaseFrame() so VisualizePose calls + # against quest-frame poses (controller markers in v11) can render. + # + # No control path uses this transform: the v11 kinematic math composes the + # controller delta in quest and applies it to the EE in world directly, + # bypassing TF entirely. The lie is contained to RViz markers. + # + # Before shipping v11, remove the quest-frame VisualizePose calls from the + # objective and delete this node — neither will be needed. + static_tf_world_to_quest = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_world_to_quest", + output="log", + arguments=[ + "0", "0", "0", # x y z (identity translation; the link is geometrically meaningless) + "0", "0", "0", # yaw pitch roll + "world", "quest", + ], + ) + + return LaunchDescription([ros_tcp_endpoint_node, static_tf_world_to_quest]) diff --git a/src/lab_sim/objectives/quest_teleop.xml b/src/lab_sim/objectives/quest_teleop.xml new file mode 100644 index 000000000..81bcf0b35 --- /dev/null +++ b/src/lab_sim/objectives/quest_teleop.xml @@ -0,0 +1,444 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +