search

kroshu / kuka_drivers

Repository containing ROS2 drivers for multiple KUKA robots
Apache License 2.0
77 stars 22 forks source link

Issues Launching Dual Arm Configuration with KUKA Drivers: Namespace and Controller Issues #198

Open ekampourakis opened 2 weeks ago

ekampourakis commented 2 weeks ago

We are experiencing significant difficulties in setting up a dual arm configuration using the provided KUKA launch files and drivers. We have both robotic arms integrated within a single URDF with the appropriate hardware systems configured, as we have additional links in the kinematic chain that are fixed and currently hardcoded.

What We Have Tried:

Despite these attempts, we have encountered persistent issues such as controllers failing to load, errors relating to missing type definitions, and namespace conflicts or the control node claiming all hardware resources from the whole URDF instead of only the namespaced one.

I will add our launch file for reference, as well as the trace logs from the command line, detailing the exact commands we used and the resulting error messages. Any insights into the proper setup or configuration adjustments that could help resolve these issues would be greatly appreciated.

Launch File:

#!/usr/bin/env python3

import os
from turtle import right
import xacro

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import OpaqueFunction
from launch_ros.actions import Node, LifecycleNode
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder

def launch_setup(context, *args, **kwargs):
    # Hardcoded arguments
    left_controller_ip = "192.168.1.200"
    right_controller_ip = "192.168.1.201"
    client_ip = "192.168.1.103"

    # Config file paths
    controllers_config = os.path.join(get_package_share_directory("project_cell"), "config","controllers.yaml")
    right_jtc_config = os.path.join(get_package_share_directory("project_cell"), "config", "right_joint_trajectory_controller_config.yaml")
    right_jic_config = os.path.join(get_package_share_directory("project_cell"), "config", "right_joint_impedance_controller_config.yaml")
    right_ec_config = os.path.join(get_package_share_directory("project_cell"), "config", "right_effort_controller_config.yaml")
    right_driver_config = os.path.join(get_package_share_directory("project_cell"), "config", "right_driver_config.yaml")
    left_jtc_config = os.path.join(get_package_share_directory("project_cell"), "config", "left_joint_trajectory_controller_config.yaml")
    left_jic_config = os.path.join(get_package_share_directory("project_cell"), "config", "left_joint_impedance_controller_config.yaml")
    left_ec_config = os.path.join(get_package_share_directory("project_cell"), "config", "left_effort_controller_config.yaml")
    left_driver_config = os.path.join(get_package_share_directory("project_cell"), "config", "left_driver_config.yaml")

    # Get URDF via xacro
    robot_urdf_xacro = os.path.join(
        get_package_share_directory('project_cell'),
        'urdf',
        'project_cell.urdf.xacro'
    )
    robot_description_xml = xacro.process_file(
        robot_urdf_xacro,
        mappings={
            "use_real": "true",
            "client_ip": client_ip,
            "left_controller_ip": left_controller_ip,
            "right_controller_ip": right_controller_ip,
        }
    ).toxml()
    robot_description = {'robot_description': robot_description_xml}

    right_control_node = Node(
        namespace="/right",
        package="kuka_drivers_core",
        executable="control_node",
        parameters=[
            robot_description,
            controllers_config,
            right_jtc_config,
            right_jic_config,
            right_ec_config,
            {"hardware_components_initial_state": {"unconfigured": ["right_system"]}}
        ]
    )

    left_control_node = Node(
        namespace="/left",
        package="kuka_drivers_core",
        executable="control_node",
        parameters=[
            robot_description,
            controllers_config,
            left_jtc_config,
            left_jic_config,
            left_ec_config,
            {"hardware_components_initial_state": {"unconfigured": ["left_system"]}}
        ]
    )

    right_robot_manager_node = LifecycleNode(
        name="robot_manager",
        namespace="/right",
        package="kuka_iiqka_eac_driver",
        executable="robot_manager_node",
        parameters=[
            right_driver_config,
            {
                "robot_model": "system",
                "controller_ip": right_controller_ip,
                "client_ip": client_ip,
            },
        ],
    )

    left_robot_manager_node = LifecycleNode(
        name="robot_manager",
        namespace="/left",
        package="kuka_iiqka_eac_driver",
        executable="robot_manager_node",
        parameters=[
            left_driver_config,
            {
                "robot_model": "system",
                "controller_ip": left_controller_ip,
                "client_ip": client_ip,
            },
        ],
    )

    right_robot_state_publisher = Node(
        namespace="/right",
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description]
    )

    left_robot_state_publisher = Node(
        namespace="/left",
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description]
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description]
    )

    def controller_spawner(controller_name, ns="/", activate=False):
        arg_list = [
            controller_name,
            "-c",
            ns + "/controller_manager",
            "-n",
            ns,
        ]
        if not activate:
            arg_list.append("--inactive")
        return Node(package="controller_manager", executable="spawner", arguments=arg_list)

    controller_names = [
        "joint_state_broadcaster",
        "joint_trajectory_controller",
        # "joint_group_impedance_controller",
        # "effort_controller",
        "control_mode_handler",
        "event_broadcaster",
    ]

    right_controller_spawners = [controller_spawner(name, "/right") for name in controller_names]

    left_controller_spawners = [controller_spawner(name, "/left") for name in controller_names]

    # === MoveIt Configurations ===
    move_group_configuration = {
        "publish_robot_description_semantic": True,
        "allow_trajectory_execution": True,
        "capabilities": "",
        "disable_capabilities": "",
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True,
        "monitor_dynamics": False,
    }

    moveit_config = MoveItConfigsBuilder("project_cell", package_name="project_cell_moveit_config").to_moveit_configs()
    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
        {"use_sim_time": False},
    ]
    move_group = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=move_group_params
    )

    rviz_config_file = PathJoinSubstitution([FindPackageShare("project_cell"), "rviz", "view.rviz"])
    rviz_parameters = [
        robot_description,
        moveit_config.planning_pipelines,
        moveit_config.robot_description_kinematics,
        {"use_sim_time": False}
    ]
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2_launch",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=rviz_parameters
    )

    nodes_to_start = [
        right_control_node,
        right_robot_manager_node,
        right_robot_state_publisher,
        left_control_node,
        left_robot_manager_node,
        left_robot_state_publisher,
        robot_state_publisher,
        # move_group,
        # rviz_node,
    ] + right_controller_spawners + left_controller_spawners

    return nodes_to_start

def generate_launch_description():
    return LaunchDescription([OpaqueFunction(function=launch_setup)])

controllers.yaml

right:
  controller_manager:
    ros__parameters:
      update_rate: 250 # Hz

      joint_trajectory_controller:
        type: joint_trajectory_controller/JointTrajectoryController
        joints:
          - right_joint_1
          - right_joint_2
          - right_joint_3
          - right_joint_4
          - right_joint_5
          - right_joint_6
        publish_rate: 250
        write_op_modes:
          - right_joint_1
          - right_joint_2
          - right_joint_3
          - right_joint_4
          - right_joint_5
          - right_joint_6

      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster
        publish_rate: 250

      joint_group_impedance_controller:
        type: kuka_controllers/JointGroupImpedanceController
      effort_controller:
        type: effort_controllers/JointGroupPositionController
      control_mode_handler:
        type: kuka_controllers/ControlModeHandler
      event_broadcaster:
        type: kuka_controllers/EventBroadcaster

  joint_trajectory_controller:
    ros__parameters:
      joints:
        - right_joint_1
        - right_joint_2
        - right_joint_3
        - right_joint_4
        - right_joint_5
        - right_joint_6
      command_interfaces:
        - position
        # - effort
        # - velocity
      state_interfaces:
        - position
        # - velocity
        # - acceleration
        # - effort
      state_publish_rate: 50.0
      action_monitor_rate: 20.0
      allow_partial_joints_goal: false
      allow_nonzero_velocity_at_trajectory_end: True
      constraints:
        stopped_velocity_tolerance: 0.2
        goal_time: 0.0
        right_joint_1: { trajectory: 0.2, goal: 0.1 }
        right_joint_2: { trajectory: 0.2, goal: 0.1 }
        right_joint_3: { trajectory: 0.2, goal: 0.1 }
        right_joint_4: { trajectory: 0.2, goal: 0.1 }
        right_joint_5: { trajectory: 0.2, goal: 0.1 }
        right_joint_6: { trajectory: 0.2, goal: 0.1 }

left:
  controller_manager:
    ros__parameters:
      update_rate: 250 # Hz

      joint_trajectory_controller:
        type: joint_trajectory_controller/JointTrajectoryController
        joints:
          - left_joint_1
          - left_joint_2
          - left_joint_3
          - left_joint_4
          - left_joint_5
          - left_joint_6
        publish_rate: 250
        write_op_modes:
          - left_joint_1
          - left_joint_2
          - left_joint_3
          - left_joint_4
          - left_joint_5
          - left_joint_6

      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster
        publish_rate: 250

      joint_group_impedance_controller:
        type: kuka_controllers/JointGroupImpedanceController
      effort_controller:
        type: effort_controllers/JointGroupPositionController
      control_mode_handler:
        type: kuka_controllers/ControlModeHandler
      event_broadcaster:
        type: kuka_controllers/EventBroadcaster

  joint_trajectory_controller:
    ros__parameters:
      joints:
        - left_joint_1
        - left_joint_2
        - left_joint_3
        - left_joint_4
        - left_joint_5
        - left_joint_6
      command_interfaces:
        - position
        # - effort
        # - velocity
      state_interfaces:
        - position
        # - velocity
        # - acceleration
        # - effort
      state_publish_rate: 50.0
      action_monitor_rate: 20.0
      allow_partial_joints_goal: false
      allow_nonzero_velocity_at_trajectory_end: True
      constraints:
        stopped_velocity_tolerance: 0.2
        goal_time: 0.0
        left_joint_1: { trajectory: 0.2, goal: 0.1 }
        left_joint_2: { trajectory: 0.2, goal: 0.1 }
        left_joint_3: { trajectory: 0.2, goal: 0.1 }
        left_joint_4: { trajectory: 0.2, goal: 0.1 }
        left_joint_5: { trajectory: 0.2, goal: 0.1 }
        left_joint_6: { trajectory: 0.2, goal: 0.1 }

Driver Config YAMLs:

robot_manager:
  ros__parameters:
    control_mode: 1
    position_controller_name: "joint_trajectory_controller"
    impedance_controller_name: "joint_group_impedance_controller"
    torque_controller_name: "effort_controller"
# Control mode enums:
#  1 - joint position control
#  2 - joint impedance control
#  3 - joint velocity control (not supported yet)
#  4 - joint torque control
# ... others are not yet supported

effort_controller:
  ros__parameters:
    joints:
    - left_joint_1
    - left_joint_2
    - left_joint_3
    - left_joint_4
    - left_joint_5
    - left_joint_6

joint_group_impedance_controller:
  ros__parameters:
    joints:
    - left_joint_1
    - left_joint_2
    - left_joint_3
    - left_joint_4
    - left_joint_5
    - left_joint_6
    - 
joint_trajectory_controller:
  ros__parameters:
    joints:
    - left_joint_1
    - left_joint_2
    - left_joint_3
    - left_joint_4
    - left_joint_5
    - left_joint_6

    command_interfaces:
      - position
    state_interfaces:
      - position
    state_publish_rate: 50.0
    action_monitor_rate: 20.0
    allow_nonzero_velocity_at_trajectory_end: true

Additionally we also have the same files loaded for the right arm joints and passed to the right control nodes.

For each arm in our URDF we load the appropriate ros2_control like so:

        <ros2_control name="${prefix}system" type="system">
            <hardware>
                <!-- Conditional loading of ros2_control configuration -->
                <xacro:if value="${use_mock}">
                    <!-- Mock (Fake) hardware-specific ros2_control configuration -->
                    <!-- This could include mock controllers or simulation interfaces that do not
                    interact with real or simulated physics engines -->
                    <plugin>kuka_mock_hardware_interface::KukaMockHardwareInterface</plugin>
                    <param name="cycle_time_ms">4</param>
                    <param name="roundtrip_time_micro">${roundtrip_time}</param>
                    <param name="mock_sensor_commands">false</param>
                    <param name="state_following_offset">0.0</param>
                </xacro:if>

                <xacro:if value="${use_gazebo}">
                    <!-- Gazebo-specific ros2_control configuration -->
                    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
                </xacro:if>

                <xacro:if value="${use_isaac}">
                    <!-- Isaac Sim-specific ros2_control configuration -->
                    <!-- Note: Isaac Sim configuration might require additional considerations -->
                    <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
                    <param name="joint_commands_topic">/joint_commands</param>
                    <param name="joint_states_topic">/joint_states</param>
                </xacro:if>

                <xacro:if value="${use_real}">
                    <!-- Real hardware-specific ros2_control configuration -->
                    <!-- Real hardware might not require specific xacro inclusions here, as it would
                    be more about launch configurations and external settings -->
                    <!-- Read QoS profile parameters -->
                    <xacro:if value="${qos_config_file == ''}">
                        <xacro:property name="qos_config"
                            value="$(find kuka_iiqka_eac_driver)/config/qos_config.yaml" />
                    </xacro:if>
                    <xacro:unless value="${qos_config_file == ''}">
                        <xacro:property name="qos_config" value="${qos_config_file}" />
                    </xacro:unless>
                    <xacro:property name="qos_config_dict" value="${xacro.load_yaml(qos_config)}" />
                    <xacro:property name="consequent_lost_packets"
                        value="${qos_config_dict['rt_packet_loss_profile']['consequent_lost_packets']}" />
                    <xacro:property name="lost_packets_in_timeframe"
                        value="${qos_config_dict['rt_packet_loss_profile']['lost_packets_in_timeframe']}" />
                    <xacro:property name="timeframe_ms"
                        value="${qos_config_dict['rt_packet_loss_profile']['timeframe_ms']}" />
                    <plugin>kuka_eac::KukaEACHardwareInterface</plugin>
                    <param name="client_ip">${client_ip}</param>
                    <param name="controller_ip">${controller_ip}</param>
                    <param name="consequent_lost_packets">${consequent_lost_packets}</param>
                    <param name="lost_packets_in_timeframe">${lost_packets_in_timeframe}</param>
                    <param name="timeframe_ms">${timeframe_ms}</param>
                </xacro:if>
            </hardware>

What we have noticed is that each control node launched claims both left_system and right_system so all the resources are claimed multiple times regardless of namespace setup and the same happens with the provided multi_robot_startup.

Below is the commands we execute and the relevant terminal trace:

$ ros2 launch project cell.launch.py
# After everything has loaded:
$ ros2 lifecycle set right/robot_manager configure
Transitioning successful
$ ros2 lifecycle set right/robot_manager activate
Transitioning successful
# The robot joint states are received and visible in RViz just fine.
$ ros2 lifecycle set left/robot_manager configure
Transitioning successful
$ ros2 lifecycle set left/robot_manager activate
Transitioning failed

Always the arm that is launched second from the launch file fails, and if the IPs are switched, the arm that previously couldn't connect, now connect normally (even though both connect individually).

The tracelog:

[INFO] [launch]: All log files can be found below /home/ekampourakis/.ros/log/2024-10-31-13-05-54-572420-ekampourakis-pc-279419
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [control_node-1]: process started with pid [279431]
[INFO] [robot_manager_node-2]: process started with pid [279433]
[INFO] [robot_state_publisher-3]: process started with pid [279435]
[INFO] [control_node-4]: process started with pid [279437]
[INFO] [robot_manager_node-5]: process started with pid [279439]
[INFO] [robot_state_publisher-6]: process started with pid [279441]
[INFO] [robot_state_publisher-7]: process started with pid [279443]
[INFO] [rviz2-8]: process started with pid [279445]
[INFO] [joint_states_merger-9]: process started with pid [279447]
[INFO] [spawner-10]: process started with pid [279449]
[INFO] [spawner-11]: process started with pid [279451]
[INFO] [spawner-12]: process started with pid [279453]
[INFO] [spawner-13]: process started with pid [279455]
[INFO] [spawner-14]: process started with pid [279457]
[INFO] [spawner-15]: process started with pid [279459]
[INFO] [spawner-16]: process started with pid [279461]
[INFO] [spawner-17]: process started with pid [279463]
[robot_state_publisher-3] [INFO] [1730372755.037230801] [right.robot_state_publisher]: got segment goizper
[robot_state_publisher-3] [INFO] [1730372755.037309976] [right.robot_state_publisher]: got segment left_base
[robot_state_publisher-3] [INFO] [1730372755.037317344] [right.robot_state_publisher]: got segment left_base_link
[robot_state_publisher-3] [INFO] [1730372755.037321851] [right.robot_state_publisher]: got segment left_flange
[robot_state_publisher-3] [INFO] [1730372755.037326148] [right.robot_state_publisher]: got segment left_link_1
[robot_state_publisher-3] [INFO] [1730372755.037330556] [right.robot_state_publisher]: got segment left_link_2
[robot_state_publisher-3] [INFO] [1730372755.037334374] [right.robot_state_publisher]: got segment left_link_3
[robot_state_publisher-3] [INFO] [1730372755.037338464] [right.robot_state_publisher]: got segment left_link_4
[robot_state_publisher-3] [INFO] [1730372755.037342478] [right.robot_state_publisher]: got segment left_link_5
[robot_state_publisher-3] [INFO] [1730372755.037346490] [right.robot_state_publisher]: got segment left_link_6
[robot_state_publisher-3] [INFO] [1730372755.037350871] [right.robot_state_publisher]: got segment left_tool0
[robot_state_publisher-3] [INFO] [1730372755.037355165] [right.robot_state_publisher]: got segment pillar
[robot_state_publisher-3] [INFO] [1730372755.037359239] [right.robot_state_publisher]: got segment right_base
[robot_state_publisher-3] [INFO] [1730372755.037363174] [right.robot_state_publisher]: got segment right_base_link
[robot_state_publisher-3] [INFO] [1730372755.037367344] [right.robot_state_publisher]: got segment right_flange
[robot_state_publisher-3] [INFO] [1730372755.037371203] [right.robot_state_publisher]: got segment right_link_1
[robot_state_publisher-3] [INFO] [1730372755.037375310] [right.robot_state_publisher]: got segment right_link_2
[robot_state_publisher-3] [INFO] [1730372755.037379246] [right.robot_state_publisher]: got segment right_link_3
[robot_state_publisher-3] [INFO] [1730372755.037382949] [right.robot_state_publisher]: got segment right_link_4
[robot_state_publisher-3] [INFO] [1730372755.037386883] [right.robot_state_publisher]: got segment right_link_5
[robot_state_publisher-3] [INFO] [1730372755.037390705] [right.robot_state_publisher]: got segment right_link_6
[robot_state_publisher-3] [INFO] [1730372755.037394761] [right.robot_state_publisher]: got segment right_tool0
[robot_state_publisher-3] [INFO] [1730372755.037398721] [right.robot_state_publisher]: got segment torso
[robot_state_publisher-3] [INFO] [1730372755.037402823] [right.robot_state_publisher]: got segment world
[robot_manager_node-5] [INFO] [1730372755.038392414] [left.robot_manager]: Control mode change requested
[robot_manager_node-5] [INFO] [1730372755.038444941] [left.robot_manager]: Successfully changed control mode to 1
[robot_manager_node-2] [INFO] [1730372755.038399470] [right.robot_manager]: Control mode change requested
[robot_manager_node-2] [INFO] [1730372755.038444975] [right.robot_manager]: Successfully changed control mode to 1
[robot_state_publisher-6] [INFO] [1730372755.040207357] [left.robot_state_publisher]: got segment goizper
[robot_state_publisher-6] [INFO] [1730372755.040260660] [left.robot_state_publisher]: got segment left_base
[robot_state_publisher-6] [INFO] [1730372755.040267381] [left.robot_state_publisher]: got segment left_base_link
[robot_state_publisher-6] [INFO] [1730372755.040272071] [left.robot_state_publisher]: got segment left_flange
[robot_state_publisher-6] [INFO] [1730372755.040276416] [left.robot_state_publisher]: got segment left_link_1
[robot_state_publisher-6] [INFO] [1730372755.040279999] [left.robot_state_publisher]: got segment left_link_2
[robot_state_publisher-6] [INFO] [1730372755.040283950] [left.robot_state_publisher]: got segment left_link_3
[robot_state_publisher-6] [INFO] [1730372755.040287932] [left.robot_state_publisher]: got segment left_link_4
[robot_state_publisher-6] [INFO] [1730372755.040292076] [left.robot_state_publisher]: got segment left_link_5
[robot_state_publisher-6] [INFO] [1730372755.040295641] [left.robot_state_publisher]: got segment left_link_6
[robot_state_publisher-6] [INFO] [1730372755.040299423] [left.robot_state_publisher]: got segment left_tool0
[robot_state_publisher-6] [INFO] [1730372755.040302947] [left.robot_state_publisher]: got segment pillar
[robot_state_publisher-6] [INFO] [1730372755.040305462] [left.robot_state_publisher]: got segment right_base
[robot_state_publisher-6] [INFO] [1730372755.040307866] [left.robot_state_publisher]: got segment right_base_link
[robot_state_publisher-6] [INFO] [1730372755.040310454] [left.robot_state_publisher]: got segment right_flange
[robot_state_publisher-6] [INFO] [1730372755.040312903] [left.robot_state_publisher]: got segment right_link_1
[robot_state_publisher-6] [INFO] [1730372755.040315426] [left.robot_state_publisher]: got segment right_link_2
[robot_state_publisher-6] [INFO] [1730372755.040317739] [left.robot_state_publisher]: got segment right_link_3
[robot_state_publisher-6] [INFO] [1730372755.040320078] [left.robot_state_publisher]: got segment right_link_4
[robot_state_publisher-6] [INFO] [1730372755.040322463] [left.robot_state_publisher]: got segment right_link_5
[robot_state_publisher-6] [INFO] [1730372755.040324798] [left.robot_state_publisher]: got segment right_link_6
[robot_state_publisher-6] [INFO] [1730372755.040327329] [left.robot_state_publisher]: got segment right_tool0
[robot_state_publisher-6] [INFO] [1730372755.040329709] [left.robot_state_publisher]: got segment torso
[robot_state_publisher-6] [INFO] [1730372755.040332176] [left.robot_state_publisher]: got segment world
[robot_state_publisher-7] [INFO] [1730372755.041276818] [robot_state_publisher]: got segment goizper
[robot_state_publisher-7] [INFO] [1730372755.041355182] [robot_state_publisher]: got segment left_base
[robot_state_publisher-7] [INFO] [1730372755.041365640] [robot_state_publisher]: got segment left_base_link
[robot_state_publisher-7] [INFO] [1730372755.041373246] [robot_state_publisher]: got segment left_flange
[robot_state_publisher-7] [INFO] [1730372755.041379944] [robot_state_publisher]: got segment left_link_1
[robot_state_publisher-7] [INFO] [1730372755.041386136] [robot_state_publisher]: got segment left_link_2
[robot_state_publisher-7] [INFO] [1730372755.041392205] [robot_state_publisher]: got segment left_link_3
[robot_state_publisher-7] [INFO] [1730372755.041398333] [robot_state_publisher]: got segment left_link_4
[robot_state_publisher-7] [INFO] [1730372755.041404482] [robot_state_publisher]: got segment left_link_5
[robot_state_publisher-7] [INFO] [1730372755.041410663] [robot_state_publisher]: got segment left_link_6
[robot_state_publisher-7] [INFO] [1730372755.041416600] [robot_state_publisher]: got segment left_tool0
[robot_state_publisher-7] [INFO] [1730372755.041422873] [robot_state_publisher]: got segment pillar
[robot_state_publisher-7] [INFO] [1730372755.041429056] [robot_state_publisher]: got segment right_base
[robot_state_publisher-7] [INFO] [1730372755.041435182] [robot_state_publisher]: got segment right_base_link
[robot_state_publisher-7] [INFO] [1730372755.041441348] [robot_state_publisher]: got segment right_flange
[robot_state_publisher-7] [INFO] [1730372755.041447266] [robot_state_publisher]: got segment right_link_1
[robot_state_publisher-7] [INFO] [1730372755.041453222] [robot_state_publisher]: got segment right_link_2
[robot_state_publisher-7] [INFO] [1730372755.041459019] [robot_state_publisher]: got segment right_link_3
[robot_state_publisher-7] [INFO] [1730372755.041464737] [robot_state_publisher]: got segment right_link_4
[robot_state_publisher-7] [INFO] [1730372755.041470536] [robot_state_publisher]: got segment right_link_5
[robot_state_publisher-7] [INFO] [1730372755.041476441] [robot_state_publisher]: got segment right_link_6
[robot_state_publisher-7] [INFO] [1730372755.041482312] [robot_state_publisher]: got segment right_tool0
[robot_state_publisher-7] [INFO] [1730372755.041488189] [robot_state_publisher]: got segment torso
[robot_state_publisher-7] [INFO] [1730372755.041493841] [robot_state_publisher]: got segment world
[control_node-1] [WARN] [1730372755.046892430] [right.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[control_node-4] [WARN] [1730372755.046892378] [left.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[control_node-1] [INFO] [1730372755.047206853] [resource_manager]: Loading hardware 'right_system' 
[control_node-4] [INFO] [1730372755.047221133] [resource_manager]: Loading hardware 'right_system' 
[control_node-1] [INFO] [1730372755.050392456] [resource_manager]: Initialize hardware 'right_system' 
[control_node-1] [INFO] [1730372755.050524903] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.201 and client ip: 192.168.1.103
[control_node-1] [INFO] [1730372755.050530016] [resource_manager]: Successful initialization of hardware 'right_system'
[control_node-1] [INFO] [1730372755.050603396] [resource_manager]: Loading hardware 'left_system' 
[control_node-1] [INFO] [1730372755.050610401] [resource_manager]: Initialize hardware 'left_system' 
[control_node-1] [INFO] [1730372755.050621315] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.200 and client ip: 192.168.1.103
[control_node-1] [INFO] [1730372755.050623672] [resource_manager]: Successful initialization of hardware 'left_system'
[control_node-1] [INFO] [1730372755.050671137] [right.controller_manager]: Setting component 'right_system' to 'unconfigured' state.
[control_node-1] [INFO] [1730372755.050693719] [resource_manager]: 'configure' hardware 'left_system' 
[control_node-4] [INFO] [1730372755.051529706] [resource_manager]: Initialize hardware 'right_system' 
[control_node-4] [INFO] [1730372755.051762011] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.201 and client ip: 192.168.1.103
[control_node-4] [INFO] [1730372755.051772845] [resource_manager]: Successful initialization of hardware 'right_system'
[control_node-4] [INFO] [1730372755.051896398] [resource_manager]: Loading hardware 'left_system' 
[control_node-4] [INFO] [1730372755.051909703] [resource_manager]: Initialize hardware 'left_system' 
[control_node-4] [INFO] [1730372755.051932199] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.200 and client ip: 192.168.1.103
[control_node-4] [INFO] [1730372755.051938167] [resource_manager]: Successful initialization of hardware 'left_system'
[control_node-4] [INFO] [1730372755.051999445] [left.controller_manager]: Setting component 'left_system' to 'unconfigured' state.
[control_node-4] [INFO] [1730372755.052029251] [resource_manager]: 'configure' hardware 'right_system' 
[control_node-4] [INFO] [1730372755.055782917] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-4] [INFO] [1730372755.055827106] [resource_manager]: Successful 'configure' of hardware 'right_system'
[control_node-4] [INFO] [1730372755.055846418] [resource_manager]: 'activate' hardware 'right_system' 
[control_node-1] [INFO] [1730372755.055817118] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-1] [INFO] [1730372755.055842781] [resource_manager]: Successful 'configure' of hardware 'left_system'
[control_node-1] [INFO] [1730372755.055856184] [resource_manager]: 'activate' hardware 'left_system' 
[spawner-13] [INFO] [1730372755.389586354] [spawner_event_broadcaster]: waiting for service /right/controller_manager/list_controllers to become available...
[spawner-11] [INFO] [1730372755.421786006] [spawner_joint_trajectory_controller]: waiting for service /right/controller_manager/list_controllers to become available...
[spawner-16] [INFO] [1730372755.429821606] [spawner_control_mode_handler]: waiting for service /left/controller_manager/list_controllers to become available...
[spawner-10] [INFO] [1730372755.431651634] [spawner_joint_state_broadcaster]: waiting for service /right/controller_manager/list_controllers to become available...
[spawner-12] [INFO] [1730372755.431959120] [spawner_control_mode_handler]: waiting for service /right/controller_manager/list_controllers to become available...
[spawner-17] [INFO] [1730372755.432735829] [spawner_event_broadcaster]: waiting for service /left/controller_manager/list_controllers to become available...
[spawner-15] [INFO] [1730372755.433116793] [spawner_joint_trajectory_controller]: waiting for service /left/controller_manager/list_controllers to become available...
[spawner-14] [INFO] [1730372755.453910079] [spawner_joint_state_broadcaster]: waiting for service /left/controller_manager/list_controllers to become available...
[rviz2-8] [INFO] [1730372755.955188853] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [INFO] [1730372755.955242581] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-8] [INFO] [1730372756.040002400] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-8]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[control_node-4] [ERROR] [1730372758.115822035] [KukaEACHardwareInterface]: Starting external control failed, error message: Could not start external control
[control_node-4] [INFO] [1730372758.115846432] [resource_manager]: Failed to 'activate' hardware 'right_system'
[control_node-1] [ERROR] [1730372758.125264630] [KukaEACHardwareInterface]: Starting external control failed, error message: Could not start external control
[control_node-1] [INFO] [1730372758.125287390] [resource_manager]: Failed to 'activate' hardware 'left_system'
[control_node-1] [INFO] [1730372758.145117146] [right.controller_manager]: Loading controller 'event_broadcaster'
[spawner-13] [INFO] [1730372758.152394842] [spawner_event_broadcaster]: Loaded event_broadcaster
[control_node-1] [INFO] [1730372758.152865333] [right.controller_manager]: Configuring controller 'event_broadcaster'
[control_node-1] [INFO] [1730372758.177048458] [right.controller_manager]: Loading controller 'joint_trajectory_controller'
[control_node-1] [WARN] [1730372758.181692429] [right.joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[spawner-11] [INFO] [1730372758.184870725] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[control_node-1] [INFO] [1730372758.185463214] [right.controller_manager]: Configuring controller 'joint_trajectory_controller'
[control_node-1] [INFO] [1730372758.185605836] [right.joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[control_node-1] [INFO] [1730372758.185626367] [right.joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position].
[control_node-1] [INFO] [1730372758.185645976] [right.joint_trajectory_controller]: Using 'splines' interpolation method.
[control_node-1] [INFO] [1730372758.186188531] [right.joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[control_node-1] [INFO] [1730372758.187962525] [right.joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[control_node-1] [INFO] [1730372758.193076705] [right.controller_manager]: Loading controller 'joint_state_broadcaster'
[control_node-1] [INFO] [1730372758.196218877] [right.controller_manager]: Loading controller 'control_mode_handler'
[spawner-10] [INFO] [1730372758.196612216] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[control_node-1] [INFO] [1730372758.200199298] [right.controller_manager]: Configuring controller 'joint_state_broadcaster'
[control_node-1] [INFO] [1730372758.200249184] [right.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-12] [INFO] [1730372758.200805977] [spawner_control_mode_handler]: Loaded control_mode_handler
[control_node-1] [INFO] [1730372758.204202117] [right.controller_manager]: Configuring controller 'control_mode_handler'
[control_node-1] [INFO] [1730372758.205500088] [right.control_mode_handler]: Control mode handler configured
[INFO] [spawner-13]: process has finished cleanly [pid 279455]
[INFO] [spawner-11]: process has finished cleanly [pid 279451]
[control_node-4] [INFO] [1730372758.435696309] [left.controller_manager]: Loading controller 'control_mode_handler'
[spawner-16] [INFO] [1730372758.442972049] [spawner_control_mode_handler]: Loaded control_mode_handler
[INFO] [spawner-12]: process has finished cleanly [pid 279453]
[control_node-4] [INFO] [1730372758.443622859] [left.controller_manager]: Loading controller 'event_broadcaster'
[INFO] [spawner-10]: process has finished cleanly [pid 279449]
[control_node-4] [INFO] [1730372758.450387697] [left.controller_manager]: Loading controller 'joint_trajectory_controller'
[spawner-17] [INFO] [1730372758.450741369] [spawner_event_broadcaster]: Loaded event_broadcaster
[control_node-4] [WARN] [1730372758.455404786] [left.joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[control_node-4] [INFO] [1730372758.458229260] [left.controller_manager]: Configuring controller 'control_mode_handler'
[spawner-15] [INFO] [1730372758.458546630] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[control_node-4] [INFO] [1730372758.459289845] [left.control_mode_handler]: Control mode handler configured
[control_node-4] [INFO] [1730372758.488825312] [left.controller_manager]: Configuring controller 'event_broadcaster'
[control_node-4] [INFO] [1730372758.490340783] [left.controller_manager]: Loading controller 'joint_state_broadcaster'
[control_node-4] [INFO] [1730372758.494396075] [left.controller_manager]: Configuring controller 'joint_trajectory_controller'
[control_node-4] [INFO] [1730372758.494496200] [left.joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[control_node-4] [INFO] [1730372758.494507044] [left.joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position].
[control_node-4] [INFO] [1730372758.494518448] [left.joint_trajectory_controller]: Using 'splines' interpolation method.
[control_node-4] [INFO] [1730372758.494894688] [left.joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[spawner-14] [INFO] [1730372758.495078135] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[control_node-4] [INFO] [1730372758.496653835] [left.joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[control_node-4] [INFO] [1730372758.498438617] [left.controller_manager]: Configuring controller 'joint_state_broadcaster'
[control_node-4] [INFO] [1730372758.498467191] [left.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[INFO] [spawner-17]: process has finished cleanly [pid 279463]
[INFO] [spawner-16]: process has finished cleanly [pid 279461]
[INFO] [spawner-15]: process has finished cleanly [pid 279459]
[INFO] [spawner-14]: process has finished cleanly [pid 279457]
[rviz2-8] [ERROR] [1730372759.095081891] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-8] [INFO] [1730372759.105970349] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-8] [INFO] [1730372759.176708585] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [ERROR] [1730372759.638908896] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-8] 
[rviz2-8] [ERROR] [1730372760.030923916] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-8] 
[rviz2-8] [WARN] [1730372760.049179716] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[control_node-1] [INFO] [1730372762.767429740] [resource_manager]: 'configure' hardware 'right_system' 
[control_node-1] [INFO] [1730372762.767440751] [right.control_mode_handler]: Control mode changed to 1
[control_node-1] [INFO] [1730372762.768927454] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-1] [INFO] [1730372762.768939475] [resource_manager]: Successful 'configure' of hardware 'right_system'
[control_node-1] [WARN] [1730372762.768959619] [resource_manager]: (hardware 'right_system'): 'state/server_state' state interface already in available list. This can happen due to multiple calls to 'configure'
[control_node-1] [WARN] [1730372762.768963991] [resource_manager]: (hardware 'right_system'): 'runtime_config/control_mode' command interface already in available list. This can happen due to multiple calls to 'configure'
[control_node-1] [INFO] [1730372762.769171533] [right.controller_manager]: Switch controller timeout is set to 0, using default 1s!
[robot_manager_node-2] [INFO] [1730372762.811093957] [right.robot_manager]: Activated control mode handler and event broadcaster
[rviz2-8] [ERROR] [1730372770.063460724] [rviz2_launch]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
[rviz2-8] Error:   Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
[rviz2-8]          at line 732 in ./src/model.cpp
[rviz2-8] [ERROR] [1730372770.067547154] [moveit_rdf_loader.rdf_loader]: Unable to parse SRDF
[rviz2-8] [ERROR] [1730372770.072634915] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Robot model not loaded
[control_node-1] [INFO] [1730372770.258447000] [resource_manager]: 'activate' hardware 'right_system' 
[control_node-4] [INFO] [1730372773.334864479] [KukaEACHardwareInterface]: External control is active
[control_node-1] [INFO] [1730372773.334923864] [KukaEACHardwareInterface]: External control is active
[control_node-1] [INFO] [1730372773.335203503] [KukaEACHardwareInterface]: External control session started successfully
[control_node-1] [INFO] [1730372773.335221654] [resource_manager]: Successful 'activate' of hardware 'right_system'
[control_node-4] [ERROR] [1730372773.341613522] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-4] [ERROR] [1730372773.341622557] [KukaEACHardwareInterface]: Execution failed
[control_node-1] [ERROR] [1730372773.341705812] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-1] [ERROR] [1730372773.341712248] [KukaEACHardwareInterface]: Execution failed
[control_node-1] [ERROR] [1730372773.341723186] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-1] [ERROR] [1730372773.341727258] [KukaEACHardwareInterface]: Could not create UDP communication, first response did not arrive (in time)
[control_node-4] [ERROR] [1730372773.341777135] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-4] [ERROR] [1730372773.341784906] [KukaEACHardwareInterface]: Could not create UDP communication, first response did not arrive (in time)
[control_node-1] [INFO] [1730372773.348803059] [KukaEACHardwareInterface]: Sending stop signal
[robot_manager_node-2] [INFO] [1730372773.389395590] [right.robot_manager]: Successfully activated controllers
[control_node-4] [INFO] [1730372781.538240594] [resource_manager]: 'configure' hardware 'left_system' 
[control_node-4] [INFO] [1730372781.538254997] [left.control_mode_handler]: Control mode changed to 1
[control_node-4] [INFO] [1730372781.539522831] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-4] [INFO] [1730372781.539532064] [resource_manager]: Successful 'configure' of hardware 'left_system'
[control_node-4] [WARN] [1730372781.539536891] [resource_manager]: (hardware 'left_system'): 'state/server_state' state interface already in available list. This can happen due to multiple calls to 'configure'
[control_node-4] [WARN] [1730372781.539541307] [resource_manager]: (hardware 'left_system'): 'runtime_config/control_mode' command interface already in available list. This can happen due to multiple calls to 'configure'
[control_node-4] [INFO] [1730372781.539745155] [left.controller_manager]: Switch controller timeout is set to 0, using default 1s!
[robot_manager_node-5] [INFO] [1730372781.581901996] [left.robot_manager]: Activated control mode handler and event broadcaster

[control_node-4] [INFO] [1730372791.551701223] [resource_manager]: 'activate' hardware 'left_system' 
[control_node-4] [ERROR] [1730372794.608321088] [KukaEACHardwareInterface]: Starting external control failed, error message: Could not start external control
[control_node-4] [INFO] [1730372794.608346861] [resource_manager]: Failed to 'activate' hardware 'left_system'
[robot_manager_node-5] [ERROR] [1730372794.608454805] [left.robot_manager]: Could not activate hardware interface
ekampourakis commented 3 days ago

We managed to load and get joint states from both robots at the same time by changing the startup.launch.py file and removing the namespace from the controller_spawner function but still cannot move both arms from MoveIt.

Both arms activate using the lifecycle command ros2 lifecycle set /right/robot_manager activate and ros2 lifecycle set /left/robot_manager activate and publish the appropriate joint states. But only the last activated control node is accepting and executing commands.

The lastly activated control node stops the other external control channel:

[control_node-3] [INFO] [1731323348.965581671] [KukaEACHardwareInterface]: External control session started successfully
[control_node-3] [INFO] [1731323348.965614517] [resource_manager]: Successful 'activate' of hardware 'right_lbr_iisy11_r1300'
[control_node-3] [INFO] [1731323348.965749008] [KukaEACHardwareInterface]: External control is active
[control_node-12] [ERROR] [1731323348.970883392] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-12] [ERROR] [1731323348.970893623] [KukaEACHardwareInterface]: Execution failed
[control_node-12] [ERROR] [1731323348.970909147] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-12] [ERROR] [1731323348.970910779] [KukaEACHardwareInterface]: Joint limits violated
[robot_manager_node-4] [INFO] [1731323348.993286244] [right.robot_manager]: Successfully activated controllers

Additionally this is also the case for the provided dual arm example launch file which doesn't include MoveIt.

An update on this issue could be very useful on moving on with the dual arm setup.

Svastits commented 2 days ago

Thanks for reporting this issue, we previously did not have problems with the dual arm setup without moveit, we could launch the example, activate both drivers and move both robot with the rqt_joint_trajectory_controller. The moveit configuration files are indeed not set up to support a dual arm scenario, we will start working on that soon and will also try to investigate what could have gone wrong without moveit

ekampourakis commented 2 days ago

Thanks for looking into it. I will provide a more detailed log for running the given multi_robot_startup.launch.py script.

We have a setup with 2 iisy11 robots and have modified the launch file with the robot models and proper IPs for our network like so:

from launch.launch_description_sources.python_launch_description_source import (
    PythonLaunchDescriptionSource,
)
from launch.actions.include_launch_description import IncludeLaunchDescription
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription

def generate_launch_description():
    test_config_dir = get_package_share_directory("kuka_iiqka_eac_driver") + "/test/config/"
    return LaunchDescription(
        [
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    [
                        get_package_share_directory("kuka_iiqka_eac_driver"),
                        "/launch/",
                        "startup_with_rviz.launch.py",
                    ]
                ),
                launch_arguments={
                    "rviz_config": f"{get_package_share_directory('iiqka_moveit_example') + '/config/multi_robot.rviz'}",  # noqa: E501
                    "namespace": "test1",
                    "controller_config": f"{test_config_dir + 'test1_ros2_controller_config.yaml'}",  # noqa: E501
                    "jtc_config": f"{test_config_dir + 'test1_joint_trajectory_controller_config.yaml'}",  # noqa: E501
                    "jic_config": f"{test_config_dir + 'test1_joint_impedance_controller_config.yaml'}",  # noqa: E501
                    "ec_config": f"{test_config_dir + 'test1_effort_controller_config.yaml'}",
                    "robot_model": "lbr_iisy11_r1300",
                    "controller_ip": "192.168.1.200",
                    "client_ip": "192.168.1.103",
                }.items(),
            ),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    [
                        get_package_share_directory("kuka_iiqka_eac_driver"),
                        "/launch/",
                        "startup.launch.py",
                    ]
                ),
                launch_arguments={
                    "namespace": "test2",
                    "controller_config": f"{test_config_dir + 'test2_ros2_controller_config.yaml'}",  # noqa: E501
                    "jtc_config": f"{test_config_dir + 'test2_joint_trajectory_controller_config.yaml'}",  # noqa: E501
                    "jic_config": f"{test_config_dir + 'test2_joint_impedance_controller_config.yaml'}",  # noqa: E501
                    "ec_config": f"{test_config_dir + 'test2_effort_controller_config.yaml'}",
                    "robot_model": "lbr_iisy11_r1300",
                    "controller_ip": "192.168.1.201",
                    "client_ip": "192.168.1.103",
                    "x": "2",
                }.items(),
            ),
        ]
    )

Some times we are able to connect to both arms by configuring and activating all the controllers like this:

ros2 lifecycle set /test1/robot_manager configure
ros2 lifecycle set /test1/robot_manager activate
ros2 lifecycle set /test2/robot_manager configure
ros2 lifecycle set /test2/robot_manager activate

After activation, both arms appear as TF in RViz but the rqt trajectory tool fails to receive the robot description for either of the controllers and crashes so we cannot control them without MoveIt.

Here is a full log of the terminal window. Please note at the end that after the activation of the test2 arm, the other external control crashes, thus causing us to lose control. This happens both on the example launch file and on our launch file with and without MoveIt.

ekampourakis@ekampourakis-pc:~$ ros2 launch iiqka_moveit_example multi_robot_startup.launch.py 
[INFO] [launch]: All log files can be found below /home/ekampourakis/.ros/log/2024-11-12-10-40-23-125620-ekampourakis-pc-27281
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [control_node-1]: process started with pid [27296]
[INFO] [robot_manager_node-2]: process started with pid [27298]
[INFO] [robot_state_publisher-3]: process started with pid [27300]
[INFO] [spawner-4]: process started with pid [27302]
[INFO] [spawner-5]: process started with pid [27304]
[INFO] [spawner-6]: process started with pid [27306]
[INFO] [spawner-7]: process started with pid [27308]
[INFO] [spawner-8]: process started with pid [27310]
[INFO] [spawner-9]: process started with pid [27312]
[INFO] [rviz2-10]: process started with pid [27314]
[INFO] [control_node-11]: process started with pid [27316]
[INFO] [robot_manager_node-12]: process started with pid [27318]
[INFO] [robot_state_publisher-13]: process started with pid [27321]
[INFO] [spawner-14]: process started with pid [27323]
[INFO] [spawner-15]: process started with pid [27325]
[INFO] [spawner-16]: process started with pid [27327]
[INFO] [spawner-17]: process started with pid [27330]
[INFO] [spawner-18]: process started with pid [27339]
[INFO] [spawner-19]: process started with pid [27346]
[robot_state_publisher-3] [INFO] [1731400823.553558590] [test1.robot_state_publisher]: got segment test1_base
[robot_state_publisher-3] [INFO] [1731400823.553624247] [test1.robot_state_publisher]: got segment test1_base_link
[robot_state_publisher-3] [INFO] [1731400823.553634676] [test1.robot_state_publisher]: got segment test1_flange
[robot_state_publisher-3] [INFO] [1731400823.553641893] [test1.robot_state_publisher]: got segment test1_link_1
[robot_state_publisher-3] [INFO] [1731400823.553648413] [test1.robot_state_publisher]: got segment test1_link_2
[robot_state_publisher-3] [INFO] [1731400823.553654834] [test1.robot_state_publisher]: got segment test1_link_3
[robot_state_publisher-3] [INFO] [1731400823.553661018] [test1.robot_state_publisher]: got segment test1_link_4
[robot_state_publisher-3] [INFO] [1731400823.553667060] [test1.robot_state_publisher]: got segment test1_link_5
[robot_state_publisher-3] [INFO] [1731400823.553673383] [test1.robot_state_publisher]: got segment test1_link_6
[robot_state_publisher-3] [INFO] [1731400823.553679455] [test1.robot_state_publisher]: got segment test1_tool0
[robot_state_publisher-3] [INFO] [1731400823.553685452] [test1.robot_state_publisher]: got segment world
[robot_manager_node-12] [INFO] [1731400823.554216127] [test2.robot_manager]: Control mode change requested
[robot_manager_node-12] [INFO] [1731400823.554253033] [test2.robot_manager]: Successfully changed control mode to 1
[robot_manager_node-2] [INFO] [1731400823.554719286] [test1.robot_manager]: Control mode change requested
[robot_manager_node-2] [INFO] [1731400823.554757688] [test1.robot_manager]: Successfully changed control mode to 1
[robot_state_publisher-13] [INFO] [1731400823.555778981] [test2.robot_state_publisher]: got segment test2_base
[robot_state_publisher-13] [INFO] [1731400823.555815739] [test2.robot_state_publisher]: got segment test2_base_link
[robot_state_publisher-13] [INFO] [1731400823.555820541] [test2.robot_state_publisher]: got segment test2_flange
[robot_state_publisher-13] [INFO] [1731400823.555823437] [test2.robot_state_publisher]: got segment test2_link_1
[robot_state_publisher-13] [INFO] [1731400823.555825792] [test2.robot_state_publisher]: got segment test2_link_2
[robot_state_publisher-13] [INFO] [1731400823.555828122] [test2.robot_state_publisher]: got segment test2_link_3
[robot_state_publisher-13] [INFO] [1731400823.555830520] [test2.robot_state_publisher]: got segment test2_link_4
[robot_state_publisher-13] [INFO] [1731400823.555832779] [test2.robot_state_publisher]: got segment test2_link_5
[robot_state_publisher-13] [INFO] [1731400823.555835254] [test2.robot_state_publisher]: got segment test2_link_6
[robot_state_publisher-13] [INFO] [1731400823.555837554] [test2.robot_state_publisher]: got segment test2_tool0
[robot_state_publisher-13] [INFO] [1731400823.555839956] [test2.robot_state_publisher]: got segment world
[control_node-1] [WARN] [1731400823.559605165] [test1.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[control_node-1] [INFO] [1731400823.559735534] [resource_manager]: Loading hardware 'test1_lbr_iisy11_r1300' 
[control_node-1] [INFO] [1731400823.562328605] [resource_manager]: Initialize hardware 'test1_lbr_iisy11_r1300' 
[control_node-1] [INFO] [1731400823.562462561] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.200 and client ip: 192.168.1.103
[control_node-1] [INFO] [1731400823.562467835] [resource_manager]: Successful initialization of hardware 'test1_lbr_iisy11_r1300'
[control_node-1] [INFO] [1731400823.562561358] [test1.controller_manager]: Setting component 'test1_lbr_iisy11_r1300' to 'unconfigured' state.
[control_node-11] [WARN] [1731400823.563798239] [test2.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[control_node-11] [INFO] [1731400823.563995706] [resource_manager]: Loading hardware 'test2_lbr_iisy11_r1300' 
[control_node-11] [INFO] [1731400823.568507603] [resource_manager]: Initialize hardware 'test2_lbr_iisy11_r1300' 
[control_node-11] [INFO] [1731400823.568747493] [KukaEACHardwareInterface]: Init successful with controller ip: 192.168.1.201 and client ip: 192.168.1.103
[control_node-11] [INFO] [1731400823.568758739] [resource_manager]: Successful initialization of hardware 'test2_lbr_iisy11_r1300'
[control_node-11] [INFO] [1731400823.568917463] [test2.controller_manager]: Setting component 'test2_lbr_iisy11_r1300' to 'unconfigured' state.
[control_node-11] [INFO] [1731400823.880169110] [test2.controller_manager]: Loading controller 'effort_controller'
[control_node-11] [INFO] [1731400823.922678957] [test2.controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-17] [INFO] [1731400823.926152177] [spawner_effort_controller]: Loaded effort_controller
[spawner-14] [INFO] [1731400823.972923796] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[control_node-1] [INFO] [1731400824.024634558] [test1.controller_manager]: Loading controller 'event_broadcaster'
[control_node-11] [INFO] [1731400824.035046988] [test2.controller_manager]: Configuring controller 'effort_controller'
[control_node-11] [INFO] [1731400824.035271691] [test2.effort_controller]: configure successful
[spawner-9] [INFO] [1731400824.037494816] [spawner_event_broadcaster]: Loaded event_broadcaster
[control_node-1] [INFO] [1731400824.037905391] [test1.controller_manager]: Configuring controller 'event_broadcaster'
[control_node-11] [INFO] [1731400824.038089105] [test2.controller_manager]: Loading controller 'control_mode_handler'
[control_node-1] [INFO] [1731400824.041980579] [test1.controller_manager]: Loading controller 'effort_controller'
[control_node-11] [INFO] [1731400824.044324828] [test2.controller_manager]: Loading controller 'joint_trajectory_controller'
[control_node-1] [INFO] [1731400824.049871645] [test1.controller_manager]: Loading controller 'joint_state_broadcaster'
[control_node-11] [WARN] [1731400824.051100278] [test2.joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[control_node-11] [INFO] [1731400824.052229382] [test2.controller_manager]: Loading controller 'joint_group_impedance_controller'
[control_node-1] [INFO] [1731400824.053844090] [test1.controller_manager]: Loading controller 'joint_trajectory_controller'
[spawner-7] [INFO] [1731400824.058635657] [spawner_effort_controller]: Loaded effort_controller
[spawner-18] [INFO] [1731400824.058813733] [spawner_control_mode_handler]: Loaded control_mode_handler
[control_node-1] [WARN] [1731400824.059320661] [test1.joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[control_node-11] [INFO] [1731400824.059387696] [test2.controller_manager]: Configuring controller 'control_mode_handler'
[control_node-11] [INFO] [1731400824.060712390] [test2.control_mode_handler]: Control mode handler configured
[spawner-4] [INFO] [1731400824.061382869] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[control_node-1] [INFO] [1731400824.061766717] [test1.controller_manager]: Loading controller 'joint_group_impedance_controller'
[control_node-1] [INFO] [1731400824.065978877] [test1.controller_manager]: Loading controller 'control_mode_handler'
[spawner-15] [INFO] [1731400824.066516323] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[control_node-11] [INFO] [1731400824.067083572] [test2.controller_manager]: Configuring controller 'joint_trajectory_controller'
[control_node-11] [INFO] [1731400824.067191664] [test2.joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[control_node-11] [INFO] [1731400824.067203947] [test2.joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position].
[control_node-11] [INFO] [1731400824.067214442] [test2.joint_trajectory_controller]: Using 'splines' interpolation method.
[control_node-11] [INFO] [1731400824.067616959] [test2.joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[control_node-11] [INFO] [1731400824.068780916] [test2.joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[control_node-1] [INFO] [1731400824.069831136] [test1.controller_manager]: Configuring controller 'effort_controller'
[control_node-1] [INFO] [1731400824.070109791] [test1.effort_controller]: configure successful
[spawner-16] [INFO] [1731400824.070363300] [spawner_joint_group_impedance_controller]: Loaded joint_group_impedance_controller
[control_node-11] [INFO] [1731400824.072314695] [test2.controller_manager]: Configuring controller 'joint_group_impedance_controller'
[control_node-11] [INFO] [1731400824.072566287] [test2.joint_group_impedance_controller]: configure successful
[control_node-1] [INFO] [1731400824.073808014] [test1.controller_manager]: Configuring controller 'joint_state_broadcaster'
[control_node-1] [INFO] [1731400824.073838863] [test1.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-5] [INFO] [1731400824.075890048] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[control_node-11] [INFO] [1731400824.076959238] [test2.controller_manager]: Configuring controller 'joint_state_broadcaster'
[control_node-11] [INFO] [1731400824.077000437] [test2.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[control_node-1] [INFO] [1731400824.077811580] [test1.controller_manager]: Configuring controller 'joint_trajectory_controller'
[control_node-1] [INFO] [1731400824.077908581] [test1.joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[control_node-1] [INFO] [1731400824.077922247] [test1.joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position].
[control_node-1] [INFO] [1731400824.077934027] [test1.joint_trajectory_controller]: Using 'splines' interpolation method.
[control_node-1] [INFO] [1731400824.078168277] [test1.joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[control_node-1] [INFO] [1731400824.079023075] [test1.joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-6] [INFO] [1731400824.080225481] [spawner_joint_group_impedance_controller]: Loaded joint_group_impedance_controller
[control_node-1] [INFO] [1731400824.082054786] [test1.controller_manager]: Configuring controller 'joint_group_impedance_controller'
[control_node-1] [INFO] [1731400824.082274502] [test1.joint_group_impedance_controller]: configure successful
[spawner-8] [INFO] [1731400824.084382495] [spawner_control_mode_handler]: Loaded control_mode_handler
[control_node-1] [INFO] [1731400824.086153326] [test1.controller_manager]: Configuring controller 'control_mode_handler'
[control_node-1] [INFO] [1731400824.087244226] [test1.control_mode_handler]: Control mode handler configured
[control_node-11] [INFO] [1731400824.152772628] [test2.controller_manager]: Loading controller 'event_broadcaster'
[spawner-19] [INFO] [1731400824.162959582] [spawner_event_broadcaster]: Loaded event_broadcaster
[control_node-11] [INFO] [1731400824.163299562] [test2.controller_manager]: Configuring controller 'event_broadcaster'
[INFO] [spawner-17]: process has finished cleanly [pid 27330]
[INFO] [spawner-16]: process has finished cleanly [pid 27327]
[INFO] [spawner-15]: process has finished cleanly [pid 27325]
[INFO] [spawner-6]: process has finished cleanly [pid 27306]
[INFO] [spawner-18]: process has finished cleanly [pid 27339]
[INFO] [spawner-9]: process has finished cleanly [pid 27312]
[INFO] [spawner-5]: process has finished cleanly [pid 27304]
[INFO] [spawner-14]: process has finished cleanly [pid 27323]
[INFO] [spawner-7]: process has finished cleanly [pid 27308]
[INFO] [spawner-8]: process has finished cleanly [pid 27310]
[INFO] [spawner-4]: process has finished cleanly [pid 27302]
[INFO] [spawner-19]: process has finished cleanly [pid 27346]

[control_node-1] [INFO] [1731400836.332271002] [test1.control_mode_handler]: Control mode changed to 1
[control_node-1] [INFO] [1731400836.332273540] [resource_manager]: 'configure' hardware 'test1_lbr_iisy11_r1300' 
[control_node-1] [INFO] [1731400836.334254661] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-1] [INFO] [1731400836.334263503] [resource_manager]: Successful 'configure' of hardware 'test1_lbr_iisy11_r1300'
[control_node-1] [INFO] [1731400836.334504312] [test1.controller_manager]: Switch controller timeout is set to 0, using default 1s!
[robot_manager_node-2] [INFO] [1731400836.358497382] [test1.robot_manager]: Activated control mode handler and event broadcaster

[control_node-1] [INFO] [1731400840.628984024] [resource_manager]: 'activate' hardware 'test1_lbr_iisy11_r1300' 
[control_node-1] [INFO] [1731400843.845685509] [KukaEACHardwareInterface]: External control session started successfully
[control_node-1] [INFO] [1731400843.845703348] [resource_manager]: Successful 'activate' of hardware 'test1_lbr_iisy11_r1300'
[control_node-1] [INFO] [1731400843.845840272] [KukaEACHardwareInterface]: External control is active
[robot_manager_node-2] [INFO] [1731400843.873284582] [test1.robot_manager]: Successfully activated controllers

[control_node-11] [INFO] [1731400849.788771678] [test2.control_mode_handler]: Control mode changed to 1
[control_node-11] [INFO] [1731400849.788785703] [resource_manager]: 'configure' hardware 'test2_lbr_iisy11_r1300' 
[control_node-11] [INFO] [1731400849.790468279] [KukaEACHardwareInterface]: Set QoS profile with 2 consequent and 1 packet losses allowed in 5000 milliseconds
[control_node-11] [INFO] [1731400849.790476440] [resource_manager]: Successful 'configure' of hardware 'test2_lbr_iisy11_r1300'
[control_node-11] [INFO] [1731400849.790664035] [test2.controller_manager]: Switch controller timeout is set to 0, using default 1s!
[control_node-1] [INFO] [1731400849.794209534] [KukaEACHardwareInterface]: External control finished
[robot_manager_node-12] [INFO] [1731400849.803584410] [test2.robot_manager]: Activated control mode handler and event broadcaster

[control_node-11] [INFO] [1731400855.662690655] [resource_manager]: 'activate' hardware 'test2_lbr_iisy11_r1300' 
[control_node-11] [INFO] [1731400858.899646691] [KukaEACHardwareInterface]: External control is active
[control_node-11] [INFO] [1731400858.900010566] [KukaEACHardwareInterface]: External control session started successfully
[control_node-11] [INFO] [1731400858.900057061] [resource_manager]: Successful 'activate' of hardware 'test2_lbr_iisy11_r1300'
[robot_manager_node-12] [INFO] [1731400858.927582787] [test2.robot_manager]: Successfully activated controllers
[control_node-11] [ERROR] [1731400860.895881274] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-11] [ERROR] [1731400860.895900683] [KukaEACHardwareInterface]: Session suspended