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"
>
-
+
-
-
+
+