Robot Cell with real robot prefix

[becketps]

Affected ROS2 Driver version(s)

actual driver ros2 humble

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

Real robot

Robot SW / URSim version(s)

Polyscope X 10.11

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

I can run the robot workcell https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/tree/main/my_robot_cell/my_robot_cell_moveit_config

with ur sim when I start my configuration via ros2 launch my_robot_cell_control ur_robot_bringup.launch.py ur_type:=ur15 robot_ip:=192.168.0.168 launch_rviz:=true use_gripper:=true gripper_mode:=real use_fake_hardware:=false launch_moveit:=true

When I do the same even with paying attention to the real robot which has another tf_prefix than UR15_ and I start the skript again I recieve this error

group-15] [ERROR] [1771278420.428464468] [moveit_robot_model.robot_model]: Joint 'UR20256700197_wrist_3_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429674536] [moveit_robot_model.robot_model]: Joint 'UR20256700197_shoulder_pan_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429700767] [moveit_robot_model.robot_model]: Joint 'UR20256700197_shoulder_lift_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429709941] [moveit_robot_model.robot_model]: Joint 'UR20256700197_elbow_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429717608] [moveit_robot_model.robot_model]: Joint 'UR20256700197_wrist_1_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429724913] [moveit_robot_model.robot_model]: Joint 'UR20256700197_wrist_2_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.429732343] [moveit_robot_model.robot_model]: Joint 'UR20256700197_wrist_3_joint' not found in model 'my_robot_cell'
[move_group-15] [ERROR] [1771278420.432107069] [moveit_robot_model.robot_model]: Joint 'UR20256700197_shoulder_pan_joint'

when adding the tf_prefix
ros2 launch my_robot_cell_control ur_robot_bringup.launch.py ur_type:=ur15 robot_ip:=192.168.0.168 launch_rviz:=true use_gripper:=true gripper_mode:=real use_fake_hardware:=false launch_moveit:=true tf_prefix:=UR20256700197_

it tells me after starting right that ur15 link is not there

ove_group-15] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-15] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-15] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-15] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-15] Loading 'move_group/MoveGroupMoveAction'...
[move_group-15] Loading 'move_group/MoveGroupPlanService'...
[move_group-15] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-15] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-15]
[move_group-15] You can start planning now!
[move_group-15]
[rviz2-7] [INFO] [1771278544.649325538] [rviz2]: Message Filter dropping message: frame 'ur15_base_link' at time 1771278524.585 for reason 'discarding message because the queue is full'
[rviz2-7] [INFO] [1771278548.617926693] [rviz2]: Message Filter dropping message: frame 'ur15_base_link' at time 1771278528.577 for reason 'discarding message because the queue is full'
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)

It started but ur15_ is missing. Somehow the confings doesn't match.

Relevant log output

"""
MoveIt move_group Launch - Verwendet die bereits laufende robot_description.
Kein eigener robot_state_publisher, kein eigener xacro-Aufruf.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python.packages import get_package_share_directory
import yaml
import os


def generate_launch_description():
    declared_arguments = [
        DeclareLaunchArgument(
            "tf_prefix",
            default_value="ur15_",
            description="TF prefix for robot links"
        ),
    ]

    def launch_setup(context, *args, **kwargs):
        tf_prefix = LaunchConfiguration("tf_prefix").perform(context)
        pkg_dir = get_package_share_directory("my_robot_cell_moveit_config")

        # ============================================================
        # SRDF via xacro (klein, braucht nur tf_prefix)
        # ============================================================
        import subprocess
        srdf_xacro_path = os.path.join(pkg_dir, "config", "my_robot_cell.srdf.xacro")
        srdf_result = subprocess.run(
            ["xacro", srdf_xacro_path, f"tf_prefix:={tf_prefix}"],
            capture_output=True, text=True
        )
        robot_description_semantic = srdf_result.stdout
        print(f"[MoveIt] SRDF generiert mit tf_prefix='{tf_prefix}'")

        # ============================================================
        # Lade sonstige MoveIt-Configs (kinematics, joint_limits, etc.)
        # ============================================================
        kinematics_file = os.path.join(pkg_dir, "config", "kinematics.yaml")
        joint_limits_file = os.path.join(pkg_dir, "config", "joint_limits.yaml")
        
        kinematics = {}
        if os.path.exists(kinematics_file):
            with open(kinematics_file, 'r') as f:
                kinematics = yaml.safe_load(f) or {}

        joint_limits = {}
        if os.path.exists(joint_limits_file):
            with open(joint_limits_file, 'r') as f:
                joint_limits_raw = yaml.safe_load(f) or {}
                joint_limits = {"robot_description_planning": joint_limits_raw}

        # ============================================================
        # move_group Node - holt robot_description vom Topic!
        # ============================================================
        move_group_params = {
            "robot_description_semantic": robot_description_semantic,
            "robot_description_kinematics": kinematics,
            "publish_robot_description_semantic": True,
            "allow_trajectory_execution": True,
            "moveit_manage_controllers": True,
            "trajectory_execution.allowed_execution_duration_scaling": 1.2,
            "trajectory_execution.allowed_goal_duration_margin": 0.5,
            "trajectory_execution.allowed_start_tolerance": 0.01,
            "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
        }

        # Controller config für MoveIt
        controllers_config = {
            "moveit_simple_controller_manager": {
                "controller_names": ["scaled_joint_trajectory_controller"],
                "scaled_joint_trajectory_controller": {
                    "type": "FollowJointTrajectory",
                    "action_ns": "follow_joint_trajectory",
                    "default": True,
                    "joints": [
                        f"{tf_prefix}shoulder_pan_joint",
                        f"{tf_prefix}shoulder_lift_joint",
                        f"{tf_prefix}elbow_joint",
                        f"{tf_prefix}wrist_1_joint",
                        f"{tf_prefix}wrist_2_joint",
                        f"{tf_prefix}wrist_3_joint",
                    ],
                },
            }
        }

        # Planning pipelines
        ompl_file = os.path.join(pkg_dir, "config", "ompl_planning.yaml")
        ompl_config = {}
        if os.path.exists(ompl_file):
            with open(ompl_file, 'r') as f:
                ompl_config = yaml.safe_load(f) or {}

        planning_params = {
            "planning_pipelines": ["ompl"],
            "default_planning_pipeline": "ompl",
        }

        move_group_node = Node(
            package="moveit_ros_move_group",
            executable="move_group",
            output="screen",
            parameters=[
                move_group_params,
                controllers_config,
                joint_limits,
                ompl_config,
                planning_params,
            ],
        )

        return [move_group_node]

    return LaunchDescription(
        declared_arguments + [OpaqueFunction(function=launch_setup)]
    )

und Xacro srdf 

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="my_robot_cell">
  
  <!-- TF Prefix Parameter -->
  <xacro:arg name="tf_prefix" default="ur15"/>
  
  <!-- Direkte String-Konkatenation (immer mit Unterstrich) -->
  <xacro:property name="prefix" value="$(arg tf_prefix)"/>

  <!-- Planning Groups -->
  <group name="ur_arm">
    <chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
  </group>

  <group name="ur_arm_wpg">
    <chain base_link="${prefix}base_link" tip_link="wpg_tcp"/>
  </group>

  <group name="gripper">
    <joint name="wpg_rail_main"/>
  </group>

  <end_effector name="gripper_ee" parent_link="wpg_tcp" group="gripper"/>

  <!-- Named States -->
  <group_state name="home" group="ur_arm">
    <joint name="${prefix}shoulder_pan_joint" value="0"/>
    <joint name="${prefix}shoulder_lift_joint" value="-1.57"/>
    <joint name="${prefix}elbow_joint" value="1.57"/>
    <joint name="${prefix}wrist_1_joint" value="-1.57"/>
    <joint name="${prefix}wrist_2_joint" value="-1.57"/>
    <joint name="${prefix}wrist_3_joint" value="0"/>
  </group_state>

  <!-- Self-Collision Pairs -->
  <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent"/>
  <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}upper_arm_link" link2="${prefix}forearm_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
  <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}flange" reason="Adjacent"/>
  <disable_collisions link1="${prefix}flange" link2="${prefix}tool0" reason="Adjacent"/>
  <disable_collisions link1="${prefix}tool0" link2="wpg_flange_mount" reason="Adjacent"/>
  <disable_collisions link1="wpg_flange_mount" link2="wpg_gripper_base_link" reason="Adjacent"/>
  
  <!-- Never collide -->
  <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}upper_arm_link" reason="Never"/>
  <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_2_link" reason="Never"/>
  <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never"/>
  <disable_collisions link1="wpg_finger_1" link2="wpg_finger_2" reason="Never"/>
  <disable_collisions link1="${prefix}tool0" link2="wpg_tcp" reason="Default"/>
  <disable_collisions link1="wpg_gripper_base_link" link2="wpg_tcp" reason="Default"/>
</robot>

Accept Public visibility

• I agree to make this context public

[urfeex]

You are referencing the tutorial's main branch, but stating that you use Humble. I assume you are using the Humble branch of the tutorials repo?

Do you also launch a driver or are you trying to use the moveit example directly with the internal ROS integration? Since you are posting this in the driver's repository, I assume, you are also starting the driver in another terminal? Please provide us with as much information as possible. What commands are running and what is their output?

Maybe try to summarize on a higher level what you want to achieve and which components you want to use to achieve that.

UR20256700197_wrist_3_joint sounds like a joint information published by the robot controller itself. When using the ROS driver with the robot, it's best not to mix it with the internal ROS interface. Either deactivate it or use a different ROS_DOMAIN_ID for the robot's internal ROS 2 integration.

Without further information, any answer I could give would be a shot in the dark.

[becketps]

Yes I am using the ur-driver by launching this coniguration here. I think it happens that the real robot proviedes the prefix which I already tried to fix. Somehow I also get 2 tf-trees. Therefore I was now looking where names does not fit. It also works when I do not start moveit but when I start the problem orcure.

import os
import tempfile

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare

from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
OpaqueFunction,
TimerAction,
SetLaunchConfiguration,
IncludeLaunchDescription,
)
from launch.conditions import IfCondition
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
PythonExpression,
)

from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes
from launch.launch_description_sources import PythonLaunchDescriptionSource # ← Falls noch nicht da
from launch_ros.descriptions import ComposableNode

def create_nodes(context, *args, **kwargs):
"""Erstellt alle Nodes."""

# =================================================================
# GET ALL CONFIGURATIONS
# =================================================================
ur_type = LaunchConfiguration("ur_type").perform(context)
robot_mode = LaunchConfiguration("robot_mode").perform(context)
robot_ip = LaunchConfiguration("robot_ip").perform(context)
use_fake_hardware = LaunchConfiguration("use_fake_hardware").perform(context)

connect_tf_trees = LaunchConfiguration("connect_tf_trees").perform(context)

tf_prefix = LaunchConfiguration("tf_prefix").perform(context)
tf_prefix = tf_prefix.strip().replace('"', '')

# Handle special keywords
if tf_prefix.lower() in ["none", "null", "empty"]:
    tf_prefix = ""
    


safety_limits = LaunchConfiguration("safety_limits").perform(context)
safety_pos_margin = LaunchConfiguration("safety_pos_margin").perform(context)
safety_k_position = LaunchConfiguration("safety_k_position").perform(context)
runtime_config_package = LaunchConfiguration("runtime_config_package").perform(context)
description_package = LaunchConfiguration("description_package").perform(context)
description_file = LaunchConfiguration("description_file").perform(context)
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands").perform(context)
controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout").perform(context)
initial_joint_controller = LaunchConfiguration("initial_joint_controller").perform(context)
activate_joint_controller = LaunchConfiguration("activate_joint_controller").perform(context)
launch_rviz = LaunchConfiguration("launch_rviz").perform(context)
headless_mode = LaunchConfiguration("headless_mode").perform(context)
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client").perform(context)
use_tool_communication = LaunchConfiguration("use_tool_communication").perform(context)
tool_parity = LaunchConfiguration("tool_parity").perform(context)
tool_baud_rate = LaunchConfiguration("tool_baud_rate").perform(context)
tool_stop_bits = LaunchConfiguration("tool_stop_bits").perform(context)
tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars").perform(context)
tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars").perform(context)
tool_device_name = LaunchConfiguration("tool_device_name").perform(context)
tool_tcp_port = LaunchConfiguration("tool_tcp_port").perform(context)
tool_voltage = LaunchConfiguration("tool_voltage").perform(context)
reverse_ip = LaunchConfiguration("reverse_ip").perform(context)
script_command_port = LaunchConfiguration("script_command_port").perform(context)
reverse_port = LaunchConfiguration("reverse_port").perform(context)
script_sender_port = LaunchConfiguration("script_sender_port").perform(context)
trajectory_port = LaunchConfiguration("trajectory_port").perform(context)

# RViz
rviz_config_package = LaunchConfiguration("rviz_config_package").perform(context)
rviz_config_file_name = LaunchConfiguration("rviz_config_file").perform(context)

# Gripper
use_gripper = LaunchConfiguration("use_gripper").perform(context)
gripper_mode = LaunchConfiguration("gripper_mode").perform(context)
gripper_ip = LaunchConfiguration("gripper_ip").perform(context)
gripper_port = LaunchConfiguration("gripper_port").perform(context)

camera_model = LaunchConfiguration("camera_model").perform(context)
keyboard_mode_str = LaunchConfiguration("keyboard_mode").perform(context)
keyboard_mode = keyboard_mode_str.lower() == "true"  # → Boolean True/False

# Visualisierung Marker rviz
use_visualizer = LaunchConfiguration("use_visualizer") 


# Handle gripper_mode "none"
if gripper_mode == "none":
    use_gripper = "false"

# fuer moveilt 
# MoveIt & Application
launch_moveit = LaunchConfiguration("launch_moveit").perform(context)
moveit_config_package = LaunchConfiguration("moveit_config_package").perform(context)
#launch_application = LaunchConfiguration("launch_application").perform(context)


# =================================================================
# GENERATE CONTROLLER YAML DIRECTLY (no xacro needed)
# =================================================================
controllers_yaml_content = f"""controller_manager:

ros__parameters:
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
speed_scaling_state_broadcaster:
type: ur_controllers/SpeedScalingStateBroadcaster
force_torque_sensor_broadcaster:
type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
scaled_joint_trajectory_controller:
type: ur_controllers/ScaledJointTrajectoryController
forward_velocity_controller:
type: velocity_controllers/JointGroupVelocityController
forward_effort_controller:
type: effort_controllers/JointGroupEffortController
forward_position_controller:
type: position_controllers/JointGroupPositionController
force_mode_controller:
type: ur_controllers/ForceModeController
freedrive_mode_controller:
type: ur_controllers/FreedriveModeController
passthrough_trajectory_controller:
type: ur_controllers/PassthroughTrajectoryController
tcp_pose_broadcaster:
type: pose_broadcaster/PoseBroadcaster
ur_configuration_controller:
type: ur_controllers/URConfigurationController
tool_contact_controller:
type: ur_controllers/ToolContactController

speed_scaling_state_broadcaster:
ros__parameters:
state_publish_rate: 100.0
tf_prefix: "{tf_prefix}"

ur_configuration_controller:
ros__parameters:
tf_prefix: "{tf_prefix}"

force_torque_sensor_broadcaster:
ros__parameters:
sensor_name: {tf_prefix}tcp_fts_sensor
state_interface_names:
- force.x
- force.y
- force.z
- torque.x
- torque.y
- torque.z
frame_id: {tf_prefix}tool0
topic_name: ft_data

joint_trajectory_controller:
ros__parameters:
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.2
goal_time: 0.0

scaled_joint_trajectory_controller:
ros__parameters:
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.2
goal_time: 0.0
speed_scaling_interface_name: {tf_prefix}speed_scaling/speed_scaling_factor

passthrough_trajectory_controller:
ros__parameters:
tf_prefix: "{tf_prefix}"
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint
state_interfaces:
- position
- velocity
speed_scaling_interface_name: {tf_prefix}speed_scaling/speed_scaling_factor

forward_velocity_controller:
ros__parameters:
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint
interface_name: velocity

forward_effort_controller:
ros__parameters:
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint
interface_name: effort

forward_position_controller:
ros__parameters:
joints:
- {tf_prefix}shoulder_pan_joint
- {tf_prefix}shoulder_lift_joint
- {tf_prefix}elbow_joint
- {tf_prefix}wrist_1_joint
- {tf_prefix}wrist_2_joint
- {tf_prefix}wrist_3_joint

force_mode_controller:
ros__parameters:
tf_prefix: "{tf_prefix}"

freedrive_mode_controller:
ros__parameters:
tf_prefix: "{tf_prefix}"

tcp_pose_broadcaster:
ros__parameters:
frame_id: {tf_prefix}base
pose_name: {tf_prefix}tcp_pose
tf:
child_frame_id: {tf_prefix}tool0_controller

tool_contact_controller:
ros__parameters:
tf_prefix: "{tf_prefix}"
"""

# Write to temporary file
temp_controllers_file = tempfile.NamedTemporaryFile(
    mode='w', 
    suffix='.yaml', 
    prefix='ur_controllers_',
    delete=False
)
temp_controllers_file.write(controllers_yaml_content)
temp_controllers_file.close()
temp_controllers_path = temp_controllers_file.name

# Get package paths
runtime_config_path = get_package_share_directory(runtime_config_package)

# Print configuration
print("=" * 60)
print("ROBOT CONFIGURATION:")
print(f"  Mode:              {robot_mode}")
print(f"  UR Type:           {ur_type}")
print(f"  TF Prefix:         '{tf_prefix}' {'(no prefix)' if tf_prefix == '' else ''}")
print(f"  Robot IP:          {robot_ip}")
print(f"  Use Fake Hardware: {use_fake_hardware}")
print(f"  Controllers YAML:  {temp_controllers_path}")
print("GRIPPER CONFIGURATION:")
print(f"  Use Gripper:       {use_gripper}")
print(f"  Gripper Mode:      {gripper_mode}")
print("RVIZ CONFIGURATION:")
print(f"  RViz Package:      {rviz_config_package}")
print(f"  RViz Config:       {rviz_config_file_name}")
print("=" * 60)

if use_gripper == "false" or gripper_mode == "none":
    print("  → Gripper DISABLED")
elif gripper_mode == "real":
    print("  → Using REAL Gripper (Griplink Hardware)")
elif gripper_mode == "simulation":
    print("  → Using SIMULATED Gripper")

# =================================================================
# PATH CONFIGURATIONS
# =================================================================
description_path = get_package_share_directory(description_package)

joint_limit_params = os.path.join(description_path, "config", ur_type, "joint_limits.yaml")
physical_params = os.path.join(description_path, "config", ur_type, "physical_parameters.yaml")
visual_params = os.path.join(description_path, "config", ur_type, "visual_parameters.yaml")
kinematics_params_file = os.path.join(description_path, "config", ur_type, "default_kinematics.yaml")

script_filename = PathJoinSubstitution(
    [FindPackageShare("ur_client_library"), "resources", "external_control.urscript"]
)
input_recipe_filename = PathJoinSubstitution(
    [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
)
output_recipe_filename = PathJoinSubstitution(
    [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
)

# =================================================================
# ROBOT DESCRIPTION (URDF)
# =================================================================
robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
        " ", "robot_ip:=", robot_ip,
        " ", "camera_model:=", camera_model,
        " ", "joint_limit_params:=", joint_limit_params,
        " ", "kinematics_params:=", kinematics_params_file,
        " ", "physical_params:=", physical_params,
        " ", "visual_params:=", visual_params,
        " ", "use_visualizer:=", use_visualizer, 
        " ", "safety_limits:=", safety_limits,
        " ", "safety_pos_margin:=", safety_pos_margin,
        " ", "safety_k_position:=", safety_k_position,
        " ", "name:=", ur_type,
        " ", "script_filename:=", script_filename,
        " ", "input_recipe_filename:=", input_recipe_filename,
        " ", "output_recipe_filename:=", output_recipe_filename,
        " ", "tf_prefix:=", tf_prefix,
        " ", "use_fake_hardware:=", use_fake_hardware,
        " ", "fake_sensor_commands:=", fake_sensor_commands,
        " ", "headless_mode:=", headless_mode,
        " ", "use_tool_communication:=", use_tool_communication,
        " ", "tool_parity:=", tool_parity,
        " ", "tool_baud_rate:=", tool_baud_rate,
        " ", "tool_stop_bits:=", tool_stop_bits,
        " ", "tool_rx_idle_chars:=", tool_rx_idle_chars,
        " ", "tool_tx_idle_chars:=", tool_tx_idle_chars,
        " ", "tool_device_name:=", tool_device_name,
        " ", "tool_tcp_port:=", tool_tcp_port,
        " ", "tool_voltage:=", tool_voltage,
        " ", "reverse_ip:=", reverse_ip,
        " ", "script_command_port:=", script_command_port,
        " ", "reverse_port:=", reverse_port,
        " ", "script_sender_port:=", script_sender_port,
        " ", "trajectory_port:=", trajectory_port,
    ]
)
robot_description = {
    "robot_description": ParameterValue(value=robot_description_content, value_type=str)
}

# =================================================================
# CONTROLLER & UPDATE RATE CONFIG
# =================================================================
update_rate_config_file = os.path.join(runtime_config_path, "config", f"{ur_type}_update_rate.yaml")

# =================================================================
# RVIZ CONFIGURATION
# =================================================================
rviz_config_file = PathJoinSubstitution(
    [FindPackageShare(rviz_config_package), "rviz", rviz_config_file_name]
)

# =================================================================
# CONTROL NODES
# =================================================================

# Fake Hardware Control Node (Simulation)
control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[
        robot_description,
        update_rate_config_file,
        temp_controllers_path,
    ],
    output="screen",
    condition=IfCondition(PythonExpression([f"'{use_fake_hardware}' == 'true'"])),
)

# Real Hardware Control Node
ur_control_node = Node(
    package="ur_robot_driver",
    executable="ur_ros2_control_node",
    parameters=[
        robot_description,
        update_rate_config_file,
        temp_controllers_path,
    ],
    output="screen",
    condition=IfCondition(PythonExpression([f"'{use_fake_hardware}' == 'false'"])),
)

# Dashboard Client (nur für echte Hardware)
dashboard_client_node = Node(
    package="ur_robot_driver",
    executable="dashboard_client",
    name="dashboard_client",
    output="screen",
    emulate_tty=True,
    parameters=[{"robot_ip": robot_ip}],
    condition=IfCondition(PythonExpression([
        f"'{launch_dashboard_client}' == 'true' and '{use_fake_hardware}' == 'false'"
    ])),
)

# Robot State Helper (nur für echte Hardware)
robot_state_helper_node = Node(
    package="ur_robot_driver",
    executable="robot_state_helper",
    name="ur_robot_state_helper",
    output="screen",
    parameters=[
        {"headless_mode": headless_mode == "true"},
        {"robot_ip": robot_ip},
    ],
    condition=IfCondition(PythonExpression([f"'{use_fake_hardware}' == 'false'"])),
)

# Tool Communication
tool_communication_node = Node(
    package="ur_robot_driver",
    executable="tool_communication.py",
    name="ur_tool_comm",
    output="screen",
    parameters=[
        {"robot_ip": robot_ip},
        {"tcp_port": int(tool_tcp_port)},
        {"device_name": tool_device_name},
    ],
    condition=IfCondition(PythonExpression([f"'{use_tool_communication}' == 'true'"])),
)

# URScript Interface
urscript_interface = Node(
    package="ur_robot_driver",
    executable="urscript_interface",
    parameters=[{"robot_ip": robot_ip}],
    output="screen",
)

# Controller Stopper (nur für echte Hardware)
controller_stopper_node = Node(
    package="ur_robot_driver",
    executable="controller_stopper_node",
    name="controller_stopper",
    output="screen",
    emulate_tty=True,
    parameters=[
        {"headless_mode": headless_mode == "true"},
        {"joint_controller_active": activate_joint_controller == "true"},
        {
            "consistent_controllers": [
                "force_torque_sensor_broadcaster",
                "joint_state_broadcaster",
                "speed_scaling_state_broadcaster",
                "tcp_pose_broadcaster",
                "ur_configuration_controller",
                "scaled_joint_trajectory_controller",
            ]
        },
    ],
    condition=IfCondition(PythonExpression([f"'{use_fake_hardware}' == 'false'"])),
)

# Robot State Publisher
robot_state_publisher_node = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    output="both",
    parameters=[robot_description],
)

world_connector = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='ur_world_connector',
    arguments=['0', '0', '0', '0', '0', '0',
            'robot_mount',
            f'{tf_prefix}world'],
    output='screen',
    condition=IfCondition(PythonExpression([f"'{connect_tf_trees}' == 'true'"])),
)

# RViz Node
rviz_node = Node(
    package="rviz2",
    executable="rviz2",
    name="rviz2",
    output="log",
    arguments=["-d", rviz_config_file],
    condition=IfCondition(PythonExpression([f"'{launch_rviz}' == 'true'"])),
)

# Trajectory Until Node
trajectory_until_node = Node(
    package="ur_robot_driver",
    executable="trajectory_until_node",
    name="trajectory_until_node",
    output="screen",
    parameters=[
        {
            "motion_controller_uri": f"/{initial_joint_controller}/follow_joint_trajectory",
            "until_action_uri": "tool_contact_controller/detect_tool_contact",
        },
    ],
)

# =================================================================
# CONTROLLER SPAWNER
# =================================================================
def controller_spawner(controllers, active=True):
    inactive_flags = ["--inactive"] if not active else []
    return Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "--controller-manager", "/controller_manager",
            "--controller-manager-timeout", controller_spawner_timeout,
        ] + inactive_flags + controllers,
    )

controllers_active = [
    "joint_state_broadcaster",
]

# Add initial_joint_controller first if it should be active
if activate_joint_controller == "true":
    controllers_active.append(initial_joint_controller)

# Then add the rest
controllers_active.extend([
    "speed_scaling_state_broadcaster",
    "force_torque_sensor_broadcaster",
    "ur_configuration_controller",
])

if use_fake_hardware == "false":
    controllers_active.append("tcp_pose_broadcaster")

controllers_inactive = [
    "joint_trajectory_controller",
    "forward_velocity_controller",
    "forward_position_controller",
    "forward_effort_controller",
    "force_mode_controller",
    "passthrough_trajectory_controller",
    "freedrive_mode_controller",
    "tool_contact_controller",
]

# Remove initial_joint_controller from inactive if it was added to active
if activate_joint_controller == "true" and initial_joint_controller in controllers_inactive:
    controllers_inactive.remove(initial_joint_controller)

# Add scaled_joint_trajectory_controller to inactive if not already active
if initial_joint_controller != "scaled_joint_trajectory_controller":
    controllers_inactive.insert(0, "scaled_joint_trajectory_controller")

controller_spawners = [
    controller_spawner(controllers_active),
    controller_spawner(controllers_inactive, active=False),
]

# =================================================================
# GRIPPER NODES
# =================================================================
gripper_is_real = use_gripper == "true" and gripper_mode == "real"
gripper_is_simulation = use_gripper == "true" and gripper_mode == "simulation"

gripper_nodes = []

if gripper_is_real:
    gripper_nodes.extend([
        TimerAction(
            period=0.5,
            actions=[
                Node(
                    package="griplink",
                    executable="griplink_node",
                    name="griplink_node",
                    namespace='griplink_node',
                    parameters=[
                        {"ip": gripper_ip},
                        {"port": int(gripper_port)},
                    ],
                    output="screen",
                )
            ]
        ),
        TimerAction(
            period=3.0,
            actions=[
                Node(
                    package='griplink_client_py',
                    executable='griplink_client',
                    name='griplink_client',
                    parameters=[{"keyboard_mode": keyboard_mode}],
                    #parameters=[{"keyboard_mode": True}],
                    output='screen'
                ),
            ]
        ),
        TimerAction(
            period=4.5,
            actions=[
                Node(
                    package="wpg_description",
                    executable="gripper_overlay_publisher.py",
                    name="gripper_overlay_publisher",
                    parameters=[{"mode": "real"}],
                    output="screen",
                ),
            ]
        ),
    ])

if gripper_is_simulation:
    gripper_nodes.extend([
        TimerAction(
            period=1.0,
            actions=[
                Node(
                    package='griplink_client_py',
                    executable='griplink_client_sim',
                    name='griplink_client_sim',
                    parameters=[
                        {"initial_position": 60.0},
                        {"keyboard_mode": keyboard_mode},
                    ],
                    output='screen'
                ),
            ]
        ),
        TimerAction(
            period=3.5,
            actions=[
                Node(
                    package='wpg_description',
                    executable='gripper_overlay_sim.py',
                    name='gripper_overlay_sim',
                    output='screen',
                ),
            ]
        ),
    ])

# =================================================================
# CAMERA NODES (wenn camera_model == zed2i)
# =================================================================
camera_nodes = []

if camera_model == "zed2i":
    camera_nodes.extend([
        # 1. Component Container für ZED
        ComposableNodeContainer(
            name='zed_container',
            namespace='zed_isaac',
            package='rclcpp_components',
            executable='component_container_mt',
            arguments=['--ros-args', '--log-level', 'info'],
            output='screen',
        ),
        
        # 2. ZED Wrapper Launch
        TimerAction(
            period=2.0,
            actions=[
                IncludeLaunchDescription(
                    launch_description_source=PythonLaunchDescriptionSource([
                        get_package_share_directory('zed_wrapper'),
                        '/launch/zed_camera.launch.py'
                    ]),
                    launch_arguments={
                        'camera_model': 'zed2i',
                        'camera_name': 'zed2i',
                        'container_name': 'zed_container',
                        'namespace': 'zed_isaac',
                        'enable_ipc': 'false',
                        'publish_tf': 'false',
                        'publish_urdf': 'false',
                        'ros_params_override_path': os.path.join(
                            get_package_share_directory('isaac_ros_apriltag'),
                            'config', 'zed_params.yaml'
                        ),
                    }.items()
                ),
            ]
        ),
        
        # 3. Image Converter + AprilTag Nodes
        TimerAction(
            period=5.0,
            actions=[
                LoadComposableNodes(
                    target_container='zed_isaac/zed_container',
                    composable_node_descriptions=[
                        ComposableNode(
                            package='isaac_ros_image_proc',
                            plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                            name='image_converter',
                            namespace='zed_isaac',
                            parameters=[{
                                'encoding_desired': 'bgr8',
                                'image_width': 1280,
                                'image_height': 720,
                                'num_blocks': 40,
                            }],
                            remappings=[
                                ('image_raw', 'zed2i/rgb/color/rect/image'),
                                ('image', 'zed2i/rgb/color/rect/image_bgr8')
                            ]
                        ),
                        ComposableNode(
                            package='isaac_ros_apriltag',
                            plugin='nvidia::isaac_ros::apriltag::AprilTagNode',
                            name='apriltag',
                            namespace='zed_isaac',
                            remappings=[
                                ('image', 'zed2i/rgb/color/rect/image_bgr8'),
                                ('camera_info', 'zed2i/rgb/color/rect/camera_info')
                            ],
                            parameters=[os.path.join(
                                get_package_share_directory('isaac_ros_apriltag'),
                                'config', 'april_tag.yaml'
                            )]
                        ),
                    ]
                ),
            ]
        ),
        
        # 4. AprilTag TF Publisher
        TimerAction(
            period=6.0,
            actions=[
                Node(
                    package="isaac_ros_apriltag",
                    executable="apriltag_tf_publisher.py",
                    name="apriltag_tf_publisher",
                    parameters=[{
                        'camera_frame': 'zed2i_left_camera_frame_optical',
                        'detection_topic': '/zed_isaac/tag_detections',
                        'tag_prefix': 'apriltag'
                    }],
                    output="screen"
                ),
            ]
        ),
    ])

# MOVEIT NODES (inline, kein separates Launch-File)
# =================================================================
moveit_nodes = []
if launch_moveit == "true":
    import subprocess
    import yaml

    print(f"[MoveIt] Starte mit tf_prefix: '{tf_prefix}'")
    pkg_moveit = get_package_share_directory(moveit_config_package)

    # SRDF generieren
    srdf_xacro_path = os.path.join(pkg_moveit, "config", "my_robot_cell.srdf.xacro")
    srdf_result = subprocess.run(
        ["xacro", srdf_xacro_path, f"tf_prefix:={tf_prefix}"],
        capture_output=True, text=True
    )
    if srdf_result.returncode != 0:
        print(f"[MoveIt] SRDF FEHLER: {srdf_result.stderr}")
    robot_description_semantic = srdf_result.stdout

    # Config-Dateien laden
    kinematics = {}
    kin_file = os.path.join(pkg_moveit, "config", "kinematics.yaml")
    if os.path.exists(kin_file):
        with open(kin_file, 'r') as f:
            kinematics = yaml.safe_load(f) or {}

    joint_limits = {}
    jl_file = os.path.join(pkg_moveit, "config", "joint_limits.yaml")
    if os.path.exists(jl_file):
        with open(jl_file, 'r') as f:
            jl_raw = yaml.safe_load(f) or {}
            joint_limits = {"robot_description_planning": jl_raw}

    ompl_config = {}
    ompl_file = os.path.join(pkg_moveit, "config", "ompl_planning.yaml")
    if os.path.exists(ompl_file):
        with open(ompl_file, 'r') as f:
            ompl_config = yaml.safe_load(f) or {}

    controllers_config = {
        "moveit_simple_controller_manager": {
            "controller_names": ["scaled_joint_trajectory_controller"],
            "scaled_joint_trajectory_controller": {
                "type": "FollowJointTrajectory",
                "action_ns": "follow_joint_trajectory",
                "default": True,
                "joints": [
                    f"{tf_prefix}shoulder_pan_joint",
                    f"{tf_prefix}shoulder_lift_joint",
                    f"{tf_prefix}elbow_joint",
                    f"{tf_prefix}wrist_1_joint",
                    f"{tf_prefix}wrist_2_joint",
                    f"{tf_prefix}wrist_3_joint",
                ],
            },
        }
    }

    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            controllers_config,
            joint_limits,
            ompl_config,          # ← YAML zuerst laden
            {                     # ← danach explizit überschreiben
                "robot_description_semantic": robot_description_semantic,
                "robot_description_kinematics": kinematics,
                "publish_robot_description": False,
                "publish_robot_description_semantic": True,
                "allow_trajectory_execution": True,
                "moveit_manage_controllers": True,
                "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
                "planning_pipelines": ["ompl"],
                "default_planning_pipeline": "ompl",
                "planning_plugin": "ompl_interface/OMPLPlanner",
                "ompl.planning_plugin": "ompl_interface/OMPLPlanner",  # ← beide Varianten
            },
        ],
    )
    moveit_nodes.append(TimerAction(period=8.0, actions=[move_group_node]))


####################################################
# Visualisierung Knoten
####################################################
visualizer_nodes = []
        
visualizer_node = Node(
    package='my_robot_cell_control',
    executable='visualizer.py',
    name='workspace_visualizer',
    output='screen',
    condition=IfCondition(use_visualizer) 
)
    
visualizer_nodes.extend([
        TimerAction(
            period=6.0,
            actions=[visualizer_node]
        ),
    ])
# =================================================================
# RETURN ALL NODES
# =================================================================

return [
    control_node,
    ur_control_node,
    dashboard_client_node,
    robot_state_helper_node,
    tool_communication_node,
    controller_stopper_node,
    urscript_interface,
    robot_state_publisher_node,
    world_connector,
    rviz_node,
    trajectory_until_node,
] + controller_spawners + gripper_nodes + camera_nodes + moveit_nodes + visualizer_nodes

def generate_launch_description():
declared_arguments = []

# UR specific arguments
declared_arguments.append(
    DeclareLaunchArgument(
        "ur_type",
        default_value="ur15",
        choices=["ur3", "ur5", "ur10", "ur3e", "ur5e", "ur7e", "ur10e", 
                 "ur12e", "ur16e", "ur8long", "ur15", "ur18", "ur20", "ur30"],
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "camera_model",
        default_value="none",
        description="Kamera-Modell (none, zed2i, ...)"
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "robot_mode",
        default_value="real",
        choices=["real", "simulation"],
        description="'real' = echter Roboter/URSim, 'simulation' = Fake Hardware",
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "connect_tf_trees",
        default_value="false",
        description="Nur für echten Roboter: Verbindet {tf_prefix}world mit robot_mount"
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "tf_prefix",
        default_value="ur15_",
        description="TF Prefix für Joint-Namen. Leer für keinen Prefix: tf_prefix:=\"\"",
    )
)

declared_arguments.append(
    DeclareLaunchArgument("robot_ip", default_value="192.168.0.168")
)
declared_arguments.append(
    DeclareLaunchArgument("use_fake_hardware", default_value="auto")
)
declared_arguments.append(
    DeclareLaunchArgument("safety_limits", default_value="true")
)
declared_arguments.append(
    DeclareLaunchArgument("safety_pos_margin", default_value="0.15")
)
declared_arguments.append(
    DeclareLaunchArgument("safety_k_position", default_value="20")
)
declared_arguments.append(
    DeclareLaunchArgument("runtime_config_package", default_value="my_robot_cell_control")
)
declared_arguments.append(
    DeclareLaunchArgument("description_package", default_value="my_robot_cell_control")
)
declared_arguments.append(
    DeclareLaunchArgument("description_file", default_value="my_robot_cell_controlled.urdf.xacro")
)
declared_arguments.append(
    DeclareLaunchArgument("rviz_config_package", default_value="my_robot_cell_control")
)
declared_arguments.append(
    DeclareLaunchArgument("rviz_config_file", default_value="view_robot.rviz")
)
declared_arguments.append(
    DeclareLaunchArgument("fake_sensor_commands", default_value="false")
)
declared_arguments.append(
    DeclareLaunchArgument("controller_spawner_timeout", default_value="10")
)
declared_arguments.append(
    DeclareLaunchArgument(
        "initial_joint_controller",
        default_value="scaled_joint_trajectory_controller",
        choices=[
            "scaled_joint_trajectory_controller",
            "joint_trajectory_controller",
            "forward_velocity_controller",
            "forward_position_controller",
            "freedrive_mode_controller",
            "passthrough_trajectory_controller",
        ],
    )
)
declared_arguments.append(
    DeclareLaunchArgument("activate_joint_controller", default_value="true")
)
declared_arguments.append(
    DeclareLaunchArgument("launch_rviz", default_value="true")
)
declared_arguments.append(
    DeclareLaunchArgument("headless_mode", default_value="false")
)
declared_arguments.append(
    DeclareLaunchArgument("launch_dashboard_client", default_value="true")
)

# Tool Communication
declared_arguments.append(DeclareLaunchArgument("use_tool_communication", default_value="false"))
declared_arguments.append(DeclareLaunchArgument("tool_parity", default_value="0"))
declared_arguments.append(DeclareLaunchArgument("tool_baud_rate", default_value="115200"))
declared_arguments.append(DeclareLaunchArgument("tool_stop_bits", default_value="1"))
declared_arguments.append(DeclareLaunchArgument("tool_rx_idle_chars", default_value="1.5"))
declared_arguments.append(DeclareLaunchArgument("tool_tx_idle_chars", default_value="3.5"))
declared_arguments.append(DeclareLaunchArgument("tool_device_name", default_value="/tmp/ttyUR"))
declared_arguments.append(DeclareLaunchArgument("tool_tcp_port", default_value="54321"))
declared_arguments.append(DeclareLaunchArgument("tool_voltage", default_value="0"))
declared_arguments.append(DeclareLaunchArgument("reverse_ip", default_value="0.0.0.0"))
declared_arguments.append(DeclareLaunchArgument("script_command_port", default_value="50004"))
declared_arguments.append(DeclareLaunchArgument("reverse_port", default_value="50001"))
declared_arguments.append(DeclareLaunchArgument("script_sender_port", default_value="50002"))
declared_arguments.append(DeclareLaunchArgument("trajectory_port", default_value="50003"))

# Gripper
declared_arguments.append(DeclareLaunchArgument("use_gripper", default_value="true"))
declared_arguments.append(DeclareLaunchArgument("gripper_ip", default_value="192.168.1.40"))
declared_arguments.append(DeclareLaunchArgument("gripper_port", default_value="10001"))
declared_arguments.append(
    DeclareLaunchArgument(
        "gripper_mode",
        default_value="auto",
        choices=["real", "simulation", "none", "auto"],
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "keyboard_mode",
        default_value="false",
        description="Enable keyboard control for gripper"
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "launch_moveit",
        default_value="false",
        description="Start MoveIt move_group"
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "moveit_config_package",
        default_value="my_robot_cell_moveit_config",
        description="MoveIt Konfigurations-Package"
    )
)

declared_arguments.append(DeclareLaunchArgument(
        'use_visualizer',
        default_value='true',
        description='Start workspace visualizer'
    )
)

declared_arguments.append(
    DeclareLaunchArgument(
        "launch_application",
        default_value="none",
        description="Welche Anwendung starten? (none, kartesian_drive, ...)"
    )
)
    
def resolve_auto_values(context, *args, **kwargs):
    robot_mode = LaunchConfiguration("robot_mode").perform(context)
    robot_ip = LaunchConfiguration("robot_ip").perform(context)
    
    use_fake_hardware_arg = LaunchConfiguration("use_fake_hardware").perform(context)
    if use_fake_hardware_arg == "auto":
        use_fake_hardware = "true" if robot_mode == "simulation" else "false"
    else:
        use_fake_hardware = use_fake_hardware_arg
    
    gripper_mode_arg = LaunchConfiguration("gripper_mode").perform(context)
    if gripper_mode_arg == "auto":
        gripper_mode = "simulation" if robot_mode == "simulation" else "real"
    else:
        gripper_mode = gripper_mode_arg
    
    # Auto-detect connect_tf_trees basierend auf robot_ip
    connect_tf_trees_arg = LaunchConfiguration("connect_tf_trees").perform(context)
    if connect_tf_trees_arg == "false":  # nur wenn nicht explizit gesetzt
        connect_tf_trees = "true" if robot_ip == "192.168.0.168" else "false"
    else:
        connect_tf_trees = connect_tf_trees_arg
    
    return [
        SetLaunchConfiguration("use_fake_hardware", use_fake_hardware),
        SetLaunchConfiguration("gripper_mode", gripper_mode),
        SetLaunchConfiguration("connect_tf_trees", connect_tf_trees),
        OpaqueFunction(function=create_nodes),
    ]

return LaunchDescription(
    declared_arguments + [
        OpaqueFunction(function=resolve_auto_values),
    ]
)

[urfeex]

Please format everything in code blocks correctly. Otherwise, things are not really readable.

As I said above: Unless you really require the internal ROS 2 interface, I would suggest deactivating it. Then you don't have two TF trees and no additional serial number prefix.

And I still don't understand what you actually want to achieve and where the tutorial example comes in here. Could you please try to explain on a higher level what you want to do?

[becketps]

My purpose is being able to drive to any position which is shown in Polyscope X. That means x, y, z and orientation.

I haven't seen in the driver move linear or something like this which directly solves the inverse kinematic and just drives like in urscript with the different move commands in karthesian space. I tried to solve the problem with moveit but the inverse kinematic solver there is not recommanded due to strange movements in total opposite direction until reaching the position without I think a lot of constraints.

Do you have a hint there which is equivalent to moveL or move process to a defined karthesian position of the TCP? I look for something like this equivalent in the driver for taking the shortest way with ellbow up configuration

Example command: movel(pose, a=1.2, v=0.25, t=0, r=0)
• Example Parameters:
• pose = p[0.2,0.3,0.5,0,0,3.14] -> position in base frame of x = 200 mm, y = 300 mm, z = 500
mm, rx = 0, ry = 0, rz = 180 deg
• a = 1.2 -> acceleration of 1.2 m/s^2
• v = 0.25 -> velocity of 250 mm/s
• t = 0 -> the time (seconds) to make the move is not specified.
• If it were specified the command would ignore the a and v values.
• r = 0 -> the blend radius is zero meters

How do I disable the Ros2 interface? Where can I check if it is enabled. I have to use the ros2 external control yes.

I faced the problem with the two trees only when enabling moveit. I think there was another node/controller which also published the wrong tree.

[urfeex]

Thank you for clarifying this. Then, I don't understand why you are using the tutorials package, at all.

I think to solve your use case, the motion primitive controller would be most suitable for you. Unfortunately, that is only available on Kilted and Rolling. You should be able to run the Rolling version of ros2_control and this driver on Humble, but you would have to compile it from source for that.

How do I disable the Ros2 interface? Where can I check if it is enabled.

https://docs.universal-robots.com/polyscopex-ros2/v10.11/Configuring%20ROS%20domain%20ID%20and%20Firewall.html

I faced the problem with the two trees only when enabling moveit.

MoveIt is the only component actively complaining about this, as far as I know. So, you might have had that state for longer, but it wasn't causing any problems so far.

[becketps]

Thanks for your hint. Is this a totally new controller or does it also use the scaled trajectory controller? Is it also usable in the code? I will build it from source for humble.

I am using the tutorial package because I have build up my own digital twin with all my components with which I have to handle cables. Yes I need to use ros2 humble.

I wanted to use the scaled joint trajectory controller and thought that it is also already supported for that motions. Then I searched of options supporting that and only found moveIT so far. I also have to integrate the planning of movements concerning on my Camera system and information extraction.

[urfeex]

The joint trajectory controller supports the joint trajectory interface as explained in https://control.ros.org/rolling/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html

If you want to make linear motions in Cartesian space, you'll have to create a joint-space trajectory matching that beforehand. That is what you tried to achieve using MoveIt. You can use many different kinematic solvers in MoveIt and also different planners. An AI agent can probably give more info about this than I can.

[becketps]

Thank you for clarifying this. Then, I don't understand why you are using the tutorials package, at all.

I think to solve your use case, the motion primitive controller would be most suitable for you. Unfortunately, that is only available on Kilted and Rolling. You should be able to run the Rolling version of ros2_control and this driver on Humble, but you would have to compile it from source for that.

How do I disable the Ros2 interface? Where can I check if it is enabled.

https://docs.universal-robots.com/polyscopex-ros2/v10.11/Configuring%20ROS%20domain%20ID%20and%20Firewall.html

I faced the problem with the two trees only when enabling moveit.

MoveIt is the only component actively complaining about this, as far as I know. So, you might have had that state for longer, but it wasn't causing any problems so far.

Is there any posiblility to disable the second tf tree? It orcures when I start moveit. Without that everything is fine an the robot also has as the standard driver the ur15_ as naming. When starting moveit the serial number comes into play with the new tree. I think it colides with the /joint_state because to be able to run it I had to remap the joint_state to joint state filtered but thats only a workaround.

[becketps]

Ok I figured out my bug in the moveit config it was.

In the moment I am asking myselft how I can avoid turning the robot each time with each way point. The normal Planner like pilz, OMPL and rrtconnect are not builded for driving linear motions of points in the near. They calculate a solution but allways turn the arm around itself.

Is there a method to drive the robot in a precise linear motion in moveIt with a normal planner? The Points on the normal ur teachpad are able to be driven linearly with the teachpad.