From d76dfb27b98cfe2c48d727e4a31fe0afcac7830c Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 10 Jun 2026 14:48:12 -0600 Subject: [PATCH] fix(kinova_site): camera streaming, USB split, and pstop recovery - config.yaml: keep joint_trajectory_controller active at startup so protective-stop recovery leaves a joint controller running; otherwise the arm settles in ARMSTATE_SERVOING_READY and the kortex driver re-reports RECOVERABLE_FAULT right after a successful reset. - ros2_control.yaml: raise JTAC path/goal tolerances to 0.20 rad so a real arm tracking a teleop path doesn't trip writeToleranceViolation (which aborts ros2_control_node via the fmt/spdlog ABI bug). - cameras.launch.xml: bump wrist-camera pub rate 6->30 Hz to match the ~30 Hz producer and keep the appsink FIFO drained (removes ~130-170 ms standing lag). - zed_camera.launch.xml: drop ZED 2i to VGA (1344x376) so two cameras fit the shared USB controller bandwidth. - robot_drivers_to_persist.launch.py: move scene_camera and scene_camera_2 onto separate USB controllers and delay scene_camera_2 by 40s to clear the wrist-camera STREAMON startup race. Co-Authored-By: Claude Opus 4.8 (1M context) --- .../config/config.yaml | 7 ++++- .../picknik_kinova_gen3.ros2_control.yaml | 6 +++++ .../launch/cameras.launch.xml | 12 +++++++-- .../launch/robot_drivers_to_persist.launch.py | 26 ++++++++++++++++--- .../launch/zed_camera.launch.xml | 20 +++++++++----- 5 files changed, 57 insertions(+), 14 deletions(-) diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/config.yaml index 1b2c49878..e218d3eff 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/config.yaml +++ b/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" diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/control/picknik_kinova_gen3.ros2_control.yaml b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/control/picknik_kinova_gen3.ros2_control.yaml index 048570da2..fe65bdb46 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/config/control/picknik_kinova_gen3.ros2_control.yaml +++ b/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: diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/cameras.launch.xml b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/cameras.launch.xml index 425fcc294..7c600f057 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/cameras.launch.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/cameras.launch.xml @@ -5,7 +5,15 @@ > - - + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/robot_drivers_to_persist.launch.py b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/robot_drivers_to_persist.launch.py index 64ffd832d..0dd620132 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/robot_drivers_to_persist.launch.py +++ b/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, ] ) diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/zed_camera.launch.xml b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/zed_camera.launch.xml index 5e2fac803..d219e4f38 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/zed_camera.launch.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_site_config/launch/zed_camera.launch.xml @@ -14,8 +14,14 @@ - - + + + - - + + @@ -60,10 +66,10 @@ namespace="$(var namespace)" output="screen" > - + - - + +