fix(kinova_site): camera streaming, USB split, and pstop recovery

src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/config.yaml

@@ -84,9 +84,14 @@ ros2_control:
     - "robotiq_gripper_controller"
     - "joint_state_broadcaster"
     - "fault_controller"
+    # Kept active so the protective-stop recovery (which deactivates everything,
+    # then reactivates this default-active set) leaves a joint-based controller
+    # running. Without it the arm settles in ARMSTATE_SERVOING_READY, which the
+    # kortex driver's read() reports as in_fault_ (hardware_interface.cpp:805) ->
+    # the arm re-reports RECOVERABLE_FAULT right after a successful reset.
+    - "joint_trajectory_controller"
   # Load but do not start these controllers so they can be activated later if needed.
   controllers_inactive_at_startup:
-    - "joint_trajectory_controller"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
     - "joint_velocity_controller"

src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/control/picknik_kinova_gen3.ros2_control.yaml

@@ -59,6 +59,12 @@ joint_trajectory_admittance_controller:
     sensor_frame: grasp_link
     ee_frame: grasp_link
     stop_accelerations: [3.0, 3.0, 3.0, 3.0, 5.0, 5.0, 5.0]
+    # Raised from the 0.05 rad (~2.9 deg) defaults: a real arm tracking a teleop-generated
+    # path easily exceeds 0.05 both along the path and at the end-point. Any violation funnels
+    # through writeToleranceViolation, which currently aborts ros2_control_node (fmt/spdlog
+    # ABI clash), so keep both generous until that controller bug is fixed upstream.
+    default_path_tolerance: 0.20
+    default_goal_tolerance: 0.20
 
 robotiq_gripper_controller:
   ros__parameters:

src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/cameras.launch.xml

@@ -5,7 +5,15 @@
   >
     <arg name="camera" value="wrist_camera" />
     <arg name="depth_registration" value="true" />
-    <arg name="max_color_pub_rate" value="6.0" />
-    <arg name="max_depth_pub_rate" value="6.0" />
+    <!-- Pub rate must keep up with the camera's ~30 Hz output. The
+         kinova_vision_node pulls ONE frame per loop iteration from a 5-deep
+         appsink FIFO (drop=true) and then sleeps at max_pub_rate. At 6 Hz the
+         consumer can't drain a 30 Hz producer, so the FIFO stays full and each
+         pull returns the OLDEST buffered frame -> a standing ~130-170 ms lag on
+         top of the RTSP/H.264 pipeline latency (total ~0.5 s vs the direct-USB
+         ZEDs). Matching 30 Hz keeps the FIFO near-empty so we get the newest
+         frame each pull. -->
+    <arg name="max_color_pub_rate" value="30.0" />
+    <arg name="max_depth_pub_rate" value="30.0" />
   </include>
 </launch>

src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/robot_drivers_to_persist.launch.py

@@ -7,7 +7,7 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
 from launch.launch_description_sources import AnyLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -60,7 +60,10 @@ def generate_launch_description():
         launch_arguments={
             "namespace": "scene_camera",
             "frame_id": "scene_camera_link",
-            "video_device": "/dev/v4l/by-path/pci-0000:0f:00.3-usb-0:1:1.0-video-index0",
+            # On a SEPARATE USB controller (pci-0000:71:00.4, dark-blue 3.2 Gen2
+            # port, SuperSpeed 5 Gbps) so the two ZED 2i don't share an xHCI
+            # controller. scene_camera_2 stays on 0b:00.0 (light-blue port).
+            "video_device": "/dev/v4l/by-path/pci-0000:71:00.4-usb-0:1:1.0-video-index0",
         }.items(),
     )
 
@@ -69,15 +72,30 @@ def generate_launch_description():
         launch_arguments={
             "namespace": "scene_camera_2",
             "frame_id": "scene_camera_2_link",
-            "video_device": "/dev/v4l/by-path/pci-0000:0f:00.4-usb-0:2:1.0-video-index0",
+            "video_device": "/dev/v4l/by-path/pci-0000:0b:00.0-usb-0:5:1.0-video-index0",
         }.items(),
     )
 
+    # Start scene_camera_2 well after the other drivers. Brought up alongside
+    # them, scene_camera_2 advertises its topics but publishes ZERO frames: its
+    # v4l2_camera_node hangs at "Starting camera" (VIDIOC_STREAMON) with no
+    # "Streaming: YES" and never retries. The device is fine (a direct
+    # `v4l2-ctl --stream-mmap` on it captures 30fps) and relaunching the node
+    # alone, once everything else is settled, always streams -- so it's a
+    # startup-contention race, not bandwidth (already mitigated by VGA) or
+    # hardware. The long pole is the wrist (kinova_vision) camera, which retries
+    # its own stream-start for ~30s before succeeding; scene_camera_2 must
+    # STREAMON after that window has cleared. 40s gives margin past the observed
+    # ~34s wrist settle. (A 10s delay was not enough -- it landed mid-storm.)
+    delayed_zed_camera_2_launch = TimerAction(
+        period=40.0, actions=[zed_camera_2_launch]
+    )
+
     return LaunchDescription(
         [
             protective_stop_manager_node,
             wrist_camera_launch,
             zed_camera_launch,
-            zed_camera_2_launch,
+            delayed_zed_camera_2_launch,
         ]
     )

src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/zed_camera.launch.xml

@@ -14,8 +14,14 @@
   <arg name="namespace" default="scene_camera" />
   <arg name="frame_id" default="scene_camera_link" />
   <arg name="video_device" default="/dev/video0" />
-  <arg name="image_width" default="2560" />
-  <arg name="image_height" default="720" />
+  <!-- VGA (1344x376 side-by-side = 672x376 per eye) is the ZED 2i's lowest UVC
+       mode. Two ZEDs at HD720 (2560x720) YUYV exceed the shared USB controller's
+       bandwidth -> neither streams (one alone works). VGA is ~1/4 the data so
+       both stream; the per-eye crop is downscaled to 224 by the Pi0 processor
+       anyway, so this matches the trained content. The camera exposes only YUYV
+       (no MJPG), so lowering resolution is the only bandwidth lever. -->
+  <arg name="image_width" default="1344" />
+  <arg name="image_height" default="376" />
 
   <node
     pkg="v4l2_camera"
@@ -43,8 +49,8 @@
   >
     <param name="offset_x" value="0" />
     <param name="offset_y" value="0" />
-    <param name="width" value="1280" />
-    <param name="height" value="720" />
+    <param name="width" value="672" />
+    <param name="height" value="376" />
     <param name="qos_overrides./$(var namespace)/left/image_raw.publisher.reliability" value="reliable" />
     <param name="qos_overrides./$(var namespace)/left/camera_info.publisher.reliability" value="reliable" />
     <remap from="in/image_raw" to="color/image_raw" />
@@ -60,10 +66,10 @@
     namespace="$(var namespace)"
     output="screen"
   >
-    <param name="offset_x" value="1280" />
+    <param name="offset_x" value="672" />
     <param name="offset_y" value="0" />
-    <param name="width" value="1280" />
-    <param name="height" value="720" />
+    <param name="width" value="672" />
+    <param name="height" value="376" />
     <param name="qos_overrides./$(var namespace)/right/image_raw.publisher.reliability" value="reliable" />
     <param name="qos_overrides./$(var namespace)/right/camera_info.publisher.reliability" value="reliable" />
     <remap from="in/image_raw" to="color/image_raw" />