Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Driver stability issue #1157

Open
1 task done
AleTarsi opened this issue Oct 17, 2024 · 5 comments
Open
1 task done

Driver stability issue #1157

AleTarsi opened this issue Oct 17, 2024 · 5 comments

Comments

@AleTarsi
Copy link

Affected ROS2 Driver version(s)

2.2.10

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.

UR E-series robot

Robot SW / URSim version(s)

5.9

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

I sporadically lose the connection with my UR5 robot, when using the ROS2 driver, but never when use_fake_hardware is set to True. The problem takes place at the driver startup and brings two possible different faults:

  1. The joint states topic is not published and the robot is not visible in Rviz, even though the robot_description topic is published together with the robot_state_publisher node. All my ros2 controllers, also are not spawned because they encounter a timeout after 30 seconds. This causes the robot to be not visible in Rviz and the easy solution to solve the problem is just restarting everything, which solves the problem almost always.
  2. The driver is partially loaded except for the ros2 controller I need to move the robot: forward position controller. This problem can be solved by either restarting all my nodes or asking again to load and activate the controller.

Issue details

I haven’t found a solution to prevent this behavior, which is annoying, especially in the first case. I tried inserting a timer in the launch file to spread the node creation overhead:

I start by launching the core nodes for the robot communication which are:

nodes_to_start = [
        control_node,
        ur_control_node,
        dashboard_client_node,
        tool_communication_node,
        controller_stopper_node,
        urscript_interface,
        robot_state_publisher_node,
    ] + controller_spawners

After 10.0 seconds I launch also MoveIt and other service nodes I need for my application in other launch files. The controller spawner timeout is currently set to 20.0 seconds.

This is the code of my launch file for the driver:

Launch file UR5 
...
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from ament_index_python.packages import get_package_share_directory
from launch.actions import (
    DeclareLaunchArgument,
    OpaqueFunction,
    TimerAction,
)
import os

def launch_setup(context, *args, **kwargs):

    # Initialize Arguments
    ur_type = LaunchConfiguration("ur_type")
    robot_ip = LaunchConfiguration("robot_ip")
    tf_prefix = LaunchConfiguration("tf_prefix")
    # General arguments
    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
    controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
    headless_mode = LaunchConfiguration("headless_mode")
    launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
    use_tool_communication = LaunchConfiguration("use_tool_communication")
    tool_device_name = LaunchConfiguration("tool_device_name")
    tool_tcp_port = LaunchConfiguration("tool_tcp_port")
    tool_voltage = LaunchConfiguration("tool_voltage")
    
    
    kinematics_params_file = os.path.join(get_package_share_directory("ur5e_gazebo"), "config", "default_kinematics.yaml")

    # # End Effector
    gripper = LaunchConfiguration("gripper")

    init_pos_file = os.path.join(
        get_package_share_directory("ur5e_gazebo"),
        "config",
        "initial_positions.yaml",
    )
    
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution([FindPackageShare("main_package"), "description", "jl2.urdf.xacro"]),
            " ","sim_gazebo:=", LaunchConfiguration("sim"),
            " ","spawn_gripper:=", gripper, 
            " ","robot_ip:=", robot_ip,
            " ","name:=", ur_type,
            " ","tf_prefix:=", tf_prefix,
            " ","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"]),
            " ","kinematics_params:=", kinematics_params_file,
            " ","use_fake_hardware:=", use_fake_hardware,
            " ","use_tool_communication:=", use_tool_communication,
            " ","tool_device_name:=", tool_device_name,
            " ","tool_tcp_port:=", tool_tcp_port,
            " ","tool_voltage:=", tool_voltage,
            " ","initial_positions_file:=", init_pos_file,
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    initial_joint_controllers = PathJoinSubstitution(
        [FindPackageShare("ur5e_gazebo"), "config", "robot_controllers.yaml"]
    )

    # define update rate
    update_rate_config_file = PathJoinSubstitution([FindPackageShare("ur_robot_driver"),"config",ur_type.perform(context) + "_update_rate.yaml"])

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            robot_description,
            update_rate_config_file,
            ParameterFile(initial_joint_controllers, allow_substs=True),
        ],
        output="screen",
        condition=IfCondition(use_fake_hardware),
    )

    ur_control_node = TimerAction(
        period=3.0,
        actions=[
            Node(
                package="ur_robot_driver",
                executable="ur_ros2_control_node",
                parameters=[
                    robot_description,
                    update_rate_config_file,
                    ParameterFile(initial_joint_controllers, allow_substs=True),
                ],
                output="screen",
                condition=UnlessCondition(use_fake_hardware),
                ),
        ]
    )
    
    dashboard_client_node = Node(
        package="ur_robot_driver",
        condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware),
        executable="dashboard_client",
        name="dashboard_client",
        output="screen",
        emulate_tty=True,
        parameters=[{"robot_ip": robot_ip}],
    )

    tool_communication_node = Node(
        package="ur_robot_driver",
        condition=IfCondition(use_tool_communication),
        executable="tool_communication.py",
        name="ur_tool_comm",
        output="screen",
        parameters=[
            {
                "robot_ip": robot_ip,
                "tcp_port": tool_tcp_port,
                "device_name": tool_device_name,
            }
        ],
    )
    
    urscript_interface = Node(
        package="ur_robot_driver",
        executable="urscript_interface",
        parameters=[{"robot_ip": robot_ip}],
        output="screen",
    )

    controller_stopper_node = TimerAction(
        period=5.0,
        actions=[
            Node(
                package="ur_robot_driver",
                executable="controller_stopper_node",
                name="controller_stopper",
                output="screen",
                emulate_tty=True,
                condition=UnlessCondition(use_fake_hardware),
                parameters=[
                    {"headless_mode": headless_mode},
                    {"joint_controller_active": activate_joint_controller},
                    {
                        "consistent_controllers": [
                            "io_and_status_controller",
                            "force_torque_sensor_broadcaster",
                            "joint_state_broadcaster",
                            "speed_scaling_state_broadcaster",
                        ]
                    },
                ],
            ),
        ]
    )

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

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

    controller_spawner_names = [
        "forward_position_controller",
        "joint_state_broadcaster",
        "io_and_status_controller",
        "speed_scaling_state_broadcaster",
        "force_torque_sensor_broadcaster",
    ]
    controller_spawner_inactive_names = ["scaled_joint_trajectory_controller"]

    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
    ]

    nodes_to_start = [
        control_node,
        ur_control_node,
        dashboard_client_node,
        tool_communication_node,
        controller_stopper_node,
        urscript_interface,
        robot_state_publisher_node,
    ] + controller_spawners
    return nodes_to_start


def generate_launch_description():
    declared_arguments = []
    # UR specific arguments
    declared_arguments.append(
        DeclareLaunchArgument(
            "ur_type",
            description="Type/series of used UR robot.",
            default_value="ur5e",
            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"],
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_ip",
            description="IP address by which the robot can be reached.",
            default_value="192.168.1.102",
        )
    )
    # General arguments
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_fake_hardware",
            default_value="false",
            description="Start robot with fake hardware mirroring command to its states.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "headless_mode",
            default_value="false",
            description="Enable headless mode for robot control",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "controller_spawner_timeout",
            default_value="20",
            description="Timeout used when spawning controllers.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "activate_joint_controller",
            default_value="true",
            description="Activate loaded joint controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tf_prefix",
            default_value="",
            description="tf_prefix of the joint names, useful for \
        multi-robot setup. If changed, also joint names in the controllers' configuration \
        have to be updated.",
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?"
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_tool_communication",
            default_value="false",
            description="Only available for e series!",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_parity",
            default_value="0",
            description="Parity configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_baud_rate",
            default_value="115200",
            description="Baud rate configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_stop_bits",
            default_value="1",
            description="Stop bits configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_rx_idle_chars",
            default_value="1.5",
            description="RX idle chars configuration for serial communication. Only effective, \
            if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_tx_idle_chars",
            default_value="3.5",
            description="TX idle chars configuration for serial communication. Only effective, \
            if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_device_name",
            default_value="/tmp/ttyUR",
            description="File descriptor that will be generated for the tool communication device. \
            The user has be be allowed to write to this location. \
            Only effective, if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_tcp_port",
            default_value="54321",
            description="Remote port that will be used for bridging the tool's serial device. \
            Only effective, if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_voltage",
            default_value="24",  # 0 being a conservative value that won't destroy anything
            description="Tool voltage that will be setup.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "reverse_ip",
            default_value="0.0.0.0",
            description="IP that will be used for the robot controller to communicate back to the driver.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "script_command_port",
            default_value="50004",
            description="Port that will be opened to forward URScript commands to the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "reverse_port",
            default_value="50001",
            description="Port that will be opened to send cyclic instructions from the driver to the robot controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "script_sender_port",
            default_value="50002",
            description="The driver will offer an interface to query the external_control URScript on this port.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "trajectory_port",
            default_value="50003",
            description="Port that will be opened for trajectory control.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "gripper",
            default_value="true",
            description="Boolean selection if you want to spawn the gripper",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "sim",
            default_value="true",
            description="Boolean selection if you want to spawn the robot in Gazebo",
        )
    )
    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

I leave the results I obtained analyzing some bag files in the different cases: the robot correctly starts, the robot doesn’t show up in Rviz, the robot doesn’t move.

Case everything works good
Case robot doesn’t spawn in Rviz
Case robot doesn’t move

Expected Behavior

The robot connection spawns correctly every time the system is launched.

Actual Behavior

Sporadically the robot doesn’t move or doesn’t show up due to nodes timeout and missing communication with the real robot.

Workaround Suggestion

Restart the application or restart missing nodes.

Relevant log output

No response

Accept Public visibility

  • I agree to make this context public
@AleTarsi
Copy link
Author

Log output in case the robot doesn’t move 
[INFO] [launch]: All log files can be found below /home/icub/.ros/log/2024-10-16-17-47-44-934091-iiticublap158u-1243222
[INFO] [launch]: Default logging verbosity is set to INFO
Using calibrated kinematics file
[INFO] [dashboard_client-2]: process started with pid [1243234]
[INFO] [ros2-1]: process started with pid [1243232]
[INFO] [urscript_interface-3]: process started with pid [1243236]
[INFO] [robot_state_publisher-4]: process started with pid [1243238]
[INFO] [controller_stopper_node-5]: process started with pid [1243240]
[dashboard_client-2] [INFO] [1729093666.527589984] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-2] 
[robot_state_publisher-4] [INFO] [1729093666.535651322] [robot_state_publisher]: got segment base
[robot_state_publisher-4] [INFO] [1729093666.535756056] [robot_state_publisher]: got segment base_link
[robot_state_publisher-4] [INFO] [1729093666.535773999] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-4] [INFO] [1729093666.535785677] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-4] [INFO] [1729093666.535794842] [robot_state_publisher]: got segment camera_link_gazebo
[robot_state_publisher-4] [INFO] [1729093666.535805006] [robot_state_publisher]: got segment camera_link_optical
[robot_state_publisher-4] [INFO] [1729093666.535821019] [robot_state_publisher]: got segment end_effector
[robot_state_publisher-4] [INFO] [1729093666.535837124] [robot_state_publisher]: got segment finger_1
[robot_state_publisher-4] [INFO] [1729093666.535849181] [robot_state_publisher]: got segment finger_2
[robot_state_publisher-4] [INFO] [1729093666.535862695] [robot_state_publisher]: got segment flange
[robot_state_publisher-4] [INFO] [1729093666.535876292] [robot_state_publisher]: got segment flange_link
[robot_state_publisher-4] [INFO] [1729093666.535890702] [robot_state_publisher]: got segment flange_to_camera
[robot_state_publisher-4] [INFO] [1729093666.535903780] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-4] [INFO] [1729093666.535918357] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-4] [INFO] [1729093666.535929662] [robot_state_publisher]: got segment rack_1
[robot_state_publisher-4] [INFO] [1729093666.535943580] [robot_state_publisher]: got segment rack_2
[robot_state_publisher-4] [INFO] [1729093666.535961318] [robot_state_publisher]: got segment root_link
[robot_state_publisher-4] [INFO] [1729093666.535977880] [robot_state_publisher]: got segment shaft
[robot_state_publisher-4] [INFO] [1729093666.535994069] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-4] [INFO] [1729093666.536008984] [robot_state_publisher]: got segment tool0
[robot_state_publisher-4] [INFO] [1729093666.536020632] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-4] [INFO] [1729093666.536032215] [robot_state_publisher]: got segment world
[robot_state_publisher-4] [INFO] [1729093666.536042521] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-4] [INFO] [1729093666.536051471] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-4] [INFO] [1729093666.536059940] [robot_state_publisher]: got segment wrist_3_link
[controller_stopper_node-5] [INFO] [1729093666.546082340] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[ros2-1] stdin is not a terminal device. Keyboard handling disabled.[INFO] [1729093666.738990768] [rosbag2_recorder]: Press SPACE for pausing/resuming
[ros2-1] [INFO] [1729093666.740628997] [rosbag2_storage]: Opened database '/media/icub/data/local_data/jl2_workspace/bag/bag_0.db3' for READ_WRITE.
[ros2-1] [INFO] [1729093666.741116313] [rosbag2_recorder]: Listening for topics...
[ros2-1] [INFO] [1729093666.741125442] [rosbag2_recorder]: Event publisher thread: Starting
[ros2-1] [INFO] [1729093666.743868077] [rosbag2_recorder]: Subscribed to topic '/rosout'
[ros2-1] [INFO] [1729093666.745802330] [rosbag2_recorder]: Subscribed to topic '/parameter_events'
[ros2-1] [INFO] [1729093666.746408648] [rosbag2_recorder]: Subscribed to topic '/events/write_split'
[ros2-1] [INFO] [1729093666.746507321] [rosbag2_recorder]: Recording...
[ros2-1] [INFO] [1729093666.849009411] [rosbag2_recorder]: Subscribed to topic '/robot_description'
[ros2-1] [INFO] [1729093666.850216449] [rosbag2_recorder]: Subscribed to topic '/tf_static'
[ros2-1] [INFO] [1729093666.850903909] [rosbag2_recorder]: Subscribed to topic '/tf'
[INFO] [ur_ros2_control_node-6]: process started with pid [1243299]
[ur_ros2_control_node-6] [WARN] [1729093669.827847064] [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.
[ur_ros2_control_node-6] text not specified in the tf_prefix tag
[ur_ros2_control_node-6] [INFO] [1729093669.828869866] [resource_manager]: Loading hardware 'ur' 
[ur_ros2_control_node-6] [INFO] [1729093669.830817040] [resource_manager]: Initialize hardware 'ur' 
[ur_ros2_control_node-6] [INFO] [1729093669.830862870] [resource_manager]: Successful initialization of hardware 'ur'
[ur_ros2_control_node-6] [INFO] [1729093669.831009246] [resource_manager]: 'configure' hardware 'ur' 
[ur_ros2_control_node-6] [INFO] [1729093669.831017195] [resource_manager]: Successful 'configure' of hardware 'ur'
[ur_ros2_control_node-6] [INFO] [1729093669.831038563] [resource_manager]: 'activate' hardware 'ur' 
[ur_ros2_control_node-6] [INFO] [1729093669.831043643] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-6] [INFO] [1729093669.831052217] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-6] [WARN] [1729093669.834111468] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-6] [INFO] [1729093669.836515687] [UR_Client_Library:]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-6] [INFO] [1729093669.838127182] [UR_Client_Library:]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-6] [WARN] [1729093670.857676977] [UR_Client_Library:]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-6] [WARN] [1729093670.857696008] [UR_Client_Library:]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-6] [INFO] [1729093670.857703132] [URPositionHardwareInterface]: Calibration checksum: 'calib_11453222953044254961'.
[ur_ros2_control_node-6] [INFO] [1729093671.869682266] [URPositionHardwareInterface]: Calibration checked successfully.
[ur_ros2_control_node-6] [INFO] [1729093671.869733611] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-6] [INFO] [1729093671.869751079] [resource_manager]: Successful 'activate' of hardware 'ur'
[controller_stopper_node-5] [INFO] [1729093671.872534071] [Controller stopper]: Service available
[controller_stopper_node-5] [INFO] [1729093671.872562864] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-5] [INFO] [1729093671.872851481] [Controller stopper]: Service available
[ur_ros2_control_node-6] [WARN] [1729093671.873670014] [controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-6] [WARN] [1729093671.873801919] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
Launching your Real Robot config in MoveIt
WARNING:root:Cannot infer URDF from `/home/icub/code/project-jl2-camozzi/jl2_workspace/install/ur5e_moveit/share/ur5e_moveit`. -- using config/jl2.urdf
WARNING:root:Cannot infer SRDF from `/home/icub/code/project-jl2-camozzi/jl2_workspace/install/ur5e_moveit/share/ur5e_moveit`. -- using config/jl2.srdf
[INFO] [move_group-7]: process started with pid [1243356]
[INFO] [rviz2-8]: process started with pid [1243358]
[INFO] [servo_pose_tracking-9]: process started with pid [1243360]
[move_group-7] [INFO] [1729093677.139134332] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00917801 seconds
[move_group-7] [INFO] [1729093677.139168289] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[servo_pose_tracking-9] [INFO] [1729093677.147745227] [moveit_servo.pose_tracking_demo]: Server Created!
[move_group-7] [WARN] [1729093677.151162287] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[move_group-7] [WARN] [1729093677.160843793] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[servo_pose_tracking-9] [INFO] [1729093677.161148811] [MoveItServoPoseTracker]: lin_tol_val: 0.001500
[servo_pose_tracking-9] [INFO] [1729093677.161193022] [MoveItServoPoseTracker]: rot_tol: 0.050000
[servo_pose_tracking-9] [INFO] [1729093677.161224984] [MoveItServoPoseTracker]: timeout: 20.000000
[servo_pose_tracking-9] [INFO] [1729093677.161244399] [MoveItServoPoseTracker]: tracker_lin_tol_val: 0.000500
[servo_pose_tracking-9] [INFO] [1729093677.161260519] [MoveItServoPoseTracker]: tracker_rot_tol: 0.030000
[servo_pose_tracking-9] [INFO] [1729093677.161276545] [MoveItServoPoseTracker]: tracker_timeout: 15.000000
[servo_pose_tracking-9] [INFO] [1729093677.180717924] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0091481 seconds
[servo_pose_tracking-9] [INFO] [1729093677.180753671] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[ros2-1] [INFO] [1729093677.185835545] [rosbag2_recorder]: Subscribed to topic '/AngleErrorEndEffector/Float32'
[ros2-1] [INFO] [1729093677.187891776] [rosbag2_recorder]: Subscribed to topic '/ErrorNormEndEffector/Float32'
[ros2-1] [INFO] [1729093677.190114250] [rosbag2_recorder]: Subscribed to topic '/TargetPoseEndEffector/TransformStamped'
[servo_pose_tracking-9] [WARN] [1729093677.191715202] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ros2-1] [INFO] [1729093677.192919351] [rosbag2_recorder]: Subscribed to topic '/target_pose'
[servo_pose_tracking-9] [WARN] [1729093677.200978081] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ros2-1] [INFO] [1729093677.201789913] [rosbag2_recorder]: Subscribed to topic '/planning_scene'
[ros2-1] [INFO] [1729093677.203167313] [rosbag2_recorder]: Subscribed to topic '/robot_description_semantic'
[move_group-7] [INFO] [1729093677.376087293] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-7] [INFO] [1729093677.376229635] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-7] [INFO] [1729093677.376812032] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-7] [INFO] [1729093677.377630973] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-7] [INFO] [1729093677.377653618] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-7] [INFO] [1729093677.379199110] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-7] [INFO] [1729093677.379224301] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-7] [INFO] [1729093677.379858944] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-7] [INFO] [1729093677.380632102] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-7] [WARN] [1729093677.381028549] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-7] [ERROR] [1729093677.381057952] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[servo_pose_tracking-9] [WARN] [1729093677.405264829] [moveit_servo.servo_parameters]: Parameter 'self_collision_proximity_threshold' should probably be less than or equal to 'scene_collision_proximity_threshold'. Check yaml file.
[move_group-7] [INFO] [1729093677.409289938] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[ros2-1] [INFO] [1729093677.411806422] [rosbag2_recorder]: Subscribed to topic '/monitored_planning_scene'
[move_group-7] [INFO] [1729093677.433718803] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-7] [INFO] [1729093677.438373702] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-7] [INFO] [1729093677.438394835] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-7] [INFO] [1729093677.438404492] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-7] [INFO] [1729093677.438435424] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-7] [INFO] [1729093677.438459897] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-7] [INFO] [1729093677.438471134] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-7] [INFO] [1729093677.438492165] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-7] [INFO] [1729093677.438503634] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-7] [INFO] [1729093677.438512566] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-7] [INFO] [1729093677.438530616] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-7] [INFO] [1729093677.438540098] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-7] [INFO] [1729093677.438546402] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-7] [INFO] [1729093677.438578741] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-7] [INFO] [1729093677.438590889] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-7] [INFO] [1729093677.440904634] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-7] [INFO] [1729093677.444906759] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-7] [INFO] [1729093677.452544799] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-7] [INFO] [1729093677.452568612] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-7] [INFO] [1729093677.454419704] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-7] [INFO] [1729093677.454442230] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-7] [INFO] [1729093677.455640099] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-7] [INFO] [1729093677.455659530] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-7] [INFO] [1729093677.456781197] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-7] [INFO] [1729093677.456798803] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-7] [INFO] [1729093677.512116527] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller
[move_group-7] [INFO] [1729093677.515675003] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller
[move_group-7] [INFO] [1729093677.515877898] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [INFO] [1729093677.515922325] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [INFO] [1729093677.516262468] [moveit_ros.trajectory_execution_manager]: Trajectory execution is not managing controllers
[move_group-7] [INFO] [1729093677.516282343] [move_group.move_group]: MoveGroup debug mode is ON
[ros2-1] [INFO] [1729093677.517968355] [rosbag2_recorder]: Subscribed to topic '/display_planned_path'
[ros2-1] [INFO] [1729093677.520320473] [rosbag2_recorder]: Subscribed to topic '/display_contacts'
[move_group-7] [INFO] [1729093677.550751017] [move_group.move_group]: 
[move_group-7] 
[move_group-7] ********************************************************
[move_group-7] * MoveGroup using: 
[move_group-7] *     - ApplyPlanningSceneService
[move_group-7] *     - ClearOctomapService
[move_group-7] *     - CartesianPathService
[move_group-7] *     - ExecuteTrajectoryAction
[move_group-7] *     - GetPlanningSceneService
[move_group-7] *     - KinematicsService
[move_group-7] *     - MoveAction
[move_group-7] *     - MotionPlanService
[move_group-7] *     - QueryPlannersService
[move_group-7] *     - StateValidationService
[move_group-7] ********************************************************
[move_group-7] 
[move_group-7] [INFO] [1729093677.550791988] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-7] [INFO] [1729093677.550802235] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-7] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-7] Loading 'move_group/ClearOctomapService'...
[move_group-7] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-7] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-7] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-7] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-7] Loading 'move_group/MoveGroupMoveAction'...
[move_group-7] Loading 'move_group/MoveGroupPlanService'...
[move_group-7] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-7] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-7] 
[move_group-7] You can start planning now!
[move_group-7] 
[ros2-1] [WARN] [1729093677.621598621] [ROSBAG2_TRANSPORT]: Hidden topics are not recorded. Enable them with --include-hidden-topics
[ros2-1] [INFO] [1729093677.626747625] [rosbag2_recorder]: Subscribed to topic '/motion_plan_request'
[rviz2-8] [INFO] [1729093677.642211705] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [INFO] [1729093677.642268995] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-8] [INFO] [1729093677.673185899] [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 /home/icub/miniforge3/envs/m2p/include/class_loader/class_loader/class_loader_core.hpp
[ros2-1] [INFO] [1729093677.940612928] [rosbag2_recorder]: Subscribed to topic '/planning_scene_world'
[INFO] [spawner-10]: process started with pid [1243419]
[INFO] [spawner-11]: process started with pid [1243421]
[INFO] [spawner-12]: process started with pid [1243423]
[INFO] [spawner-13]: process started with pid [1243425]
[INFO] [spawner-14]: process started with pid [1243427]
[INFO] [spawner-15]: process started with pid [1243429]
[ur_ros2_control_node-6] [INFO] [1729093679.030177624] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[spawner-14] [INFO] [1729093679.088470179] [spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-6] [INFO] [1729093679.090464644] [controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[ros2-1] [INFO] [1729093679.101362526] [rosbag2_recorder]: Subscribed to topic '/force_torque_sensor_broadcaster/wrench'
[spawner-14] [INFO] [1729093679.102421069] [spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[ros2-1] [INFO] [1729093679.103968544] [rosbag2_recorder]: Subscribed to topic '/force_torque_sensor_broadcaster/transition_event'
[ur_ros2_control_node-6] [INFO] [1729093679.164822751] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-6] [INFO] [1729093679.179196240] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: 
[spawner-13] [INFO] [1729093679.182558578] [spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-6] [INFO] [1729093679.183585792] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ur_ros2_control_node-6] [INFO] [1729093679.196458660] [controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-6] [INFO] [1729093679.196557496] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[spawner-11] [INFO] [1729093679.198144667] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ur_ros2_control_node-6] [INFO] [1729093679.200790967] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ur_ros2_control_node-6] [INFO] [1729093679.200884575] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ros2-1] [INFO] [1729093679.214336668] [rosbag2_recorder]: Subscribed to topic '/joint_states'
[spawner-13] [INFO] [1729093679.215546260] [spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[ros2-1] [INFO] [1729093679.216682257] [rosbag2_recorder]: Subscribed to topic '/speed_scaling_state_broadcaster/transition_event'
[ros2-1] [INFO] [1729093679.218751505] [rosbag2_recorder]: Subscribed to topic '/dynamic_joint_states'
[ros2-1] [INFO] [1729093679.220189380] [rosbag2_recorder]: Subscribed to topic '/joint_state_broadcaster/transition_event'
[ros2-1] [INFO] [1729093679.221351584] [rosbag2_recorder]: Subscribed to topic '/speed_scaling_state_broadcaster/speed_scaling'
[spawner-11] [INFO] [1729093679.231104207] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ur_ros2_control_node-6] [INFO] [1729093679.231548501] [controller_manager]: Loading controller 'io_and_status_controller'
[ur_ros2_control_node-6] [INFO] [1729093679.245017074] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[spawner-12] [INFO] [1729093679.249462216] [spawner_io_and_status_controller]: Loaded io_and_status_controller
[ur_ros2_control_node-6] [WARN] [1729093679.264858417] [scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ur_ros2_control_node-6] [INFO] [1729093679.267188500] [controller_manager]: Configuring controller 'io_and_status_controller'
[spawner-15] [INFO] [1729093679.268457123] [spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-6] [INFO] [1729093679.271571923] [controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-6] [INFO] [1729093679.271831439] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-6] [INFO] [1729093679.271892527] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-6] [INFO] [1729093679.271924580] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[ur_ros2_control_node-6] [INFO] [1729093679.272458620] [scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-6] [INFO] [1729093679.275391973] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-12] [INFO] [1729093679.295392859] [spawner_io_and_status_controller]: Configured and activated io_and_status_controller
[ros2-1] [INFO] [1729093679.330637863] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/transition_event'
[ros2-1] [INFO] [1729093679.332036997] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/transition_event'
[ros2-1] [INFO] [1729093679.333857861] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/io_states'
[ros2-1] [INFO] [1729093679.336415940] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/robot_mode'
[ros2-1] [INFO] [1729093679.338582799] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/robot_program_running'
[ros2-1] [INFO] [1729093679.341568368] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/state'
[ros2-1] [INFO] [1729093679.343717124] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/tool_data'
[ros2-1] [INFO] [1729093679.346952786] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/controller_state'
[ros2-1] [INFO] [1729093679.348841915] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/safety_mode'
[INFO] [spawner-14]: process has finished cleanly [pid 1243427]
[INFO] [spawner-15]: process has finished cleanly [pid 1243429]
[INFO] [spawner-11]: process has finished cleanly [pid 1243421]
[INFO] [spawner-13]: process has finished cleanly [pid 1243425]
[INFO] [spawner-12]: process has finished cleanly [pid 1243423]
[rviz2-8] [ERROR] [1729093680.845123184] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-8] [INFO] [1729093680.866704025] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-8] [WARN] [1729093680.899224893] [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.
[ros2-1] [INFO] [1729093680.962598666] [rosbag2_recorder]: Subscribed to topic '/clicked_point'
[ros2-1] [INFO] [1729093680.963820812] [rosbag2_recorder]: Subscribed to topic '/goal_pose'
[ros2-1] [INFO] [1729093680.965373446] [rosbag2_recorder]: Subscribed to topic '/rviz_visual_tools_gui'
[ros2-1] [INFO] [1729093680.966849634] [rosbag2_recorder]: Subscribed to topic '/initialpose'
[rviz2-8] [INFO] [1729093681.324396368] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00671747 seconds
[rviz2-8] [INFO] [1729093681.324427590] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[rviz2-8] [WARN] [1729093681.334572324] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[rviz2-8] [WARN] [1729093681.342937914] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[rviz2-8] [ERROR] [1729093681.359481751] [moveit_background_processing.background_processing]: Exception caught while processing action 'loadRobotModel': parameter 'robot_description_planning.joint_limits.shoulder_pan_joint.max_position' has invalid type: Wrong parameter type, parameter {robot_description_planning.joint_limits.shoulder_pan_joint.max_position} is of type {double}, setting it to {string} is not allowed.
[rviz2-8] [INFO] [1729093681.381765517] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [INFO] [1729093681.412508606] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [INFO] [1729093681.446807715] [rviz2]: Stereo is NOT SUPPORTED
Launching real robot
UseCase1: OPE algorithm to be launched: m2p
M2P will estimate the pose of objects with ID: 50-6512-1458
[INFO] [srv_Camera.py-16]: process started with pid [1243508]
[INFO] [srv_GraspPlanner.py-17]: process started with pid [1243510]
[INFO] [srv_GripperReal.py-18]: process started with pid [1243512]
[INFO] [service_ope.py-19]: process started with pid [1243514]
[srv_Camera.py-16] [INFO] [1729093683.166117007] [camera_server]: Simulated Gazebo: False
[ros2-1] [INFO] [1729093683.264140285] [rosbag2_recorder]: Subscribed to topic '/camera/rgb_image'
[ros2-1] [INFO] [1729093683.265629421] [rosbag2_recorder]: Subscribed to topic '/camera/depth_image'
[srv_GraspPlanner.py-17] [INFO] [1729093683.775107515] [Grasp_service]: Waiting for the transformation between camera_link_optical and end_effector...
[srv_GraspPlanner.py-17] [INFO] [1729093683.779852697] [Grasp_service]: Got the transformation between camera_link_optical and end_effector!
[servo_pose_tracking-9] [INFO] [1729093687.407166911] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[servo_pose_tracking-9] [INFO] [1729093687.409098752] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[servo_pose_tracking-9] [INFO] [1729093687.409128501] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[servo_pose_tracking-9] [INFO] [1729093687.410494656] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[servo_pose_tracking-9] [INFO] [1729093687.411850433] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[servo_pose_tracking-9] [INFO] [1729093687.413261699] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[servo_pose_tracking-9] [INFO] [1729093687.414819051] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[servo_pose_tracking-9] [INFO] [1729093687.417322689] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[ros2-1] [INFO] [1729093687.473807653] [rosbag2_recorder]: Subscribed to topic '/camera/pose_image'
[service_ope.py-19] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:500: UserWarning: The value of the smallest subnormal for <class 'numpy.float64'> type is zero.
[service_ope.py-19]   setattr(self, word, getattr(machar, word).flat[0])
[service_ope.py-19] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:89: UserWarning: The value of the smallest subnormal for <class 'numpy.float64'> type is zero.
[service_ope.py-19]   return self._float_to_str(self.smallest_subnormal)
[service_ope.py-19] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:500: UserWarning: The value of the smallest subnormal for <class 'numpy.float32'> type is zero.
[service_ope.py-19]   setattr(self, word, getattr(machar, word).flat[0])
[service_ope.py-19] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:89: UserWarning: The value of the smallest subnormal for <class 'numpy.float32'> type is zero.
[service_ope.py-19]   return self._float_to_str(self.smallest_subnormal)
[service_ope.py-19] => Templates are already in the right dir!
[service_ope.py-19] Blender 3.5.1 (hash e1ccd9d4a1d3 built 2023-04-24 23:31:15)
[service_ope.py-19] Read prefs: /home/icub/.config/blender/3.5/config/userpref.blend
[service_ope.py-19] 
[service_ope.py-19] Blender quit
[service_ope.py-19] Warning: Changed install path from /home_local/icub... to /home/icub..., there is no /home_local/ on this machine.
[service_ope.py-19] Using blender in /home/icub/blender/blender-3.5.1-linux-x64
[service_ope.py-19] Using temporary directory: /dev/shm/blender_proc_530c53a1943f4e239b173db96d0d68c0
[service_ope.py-19] Cleaning temporary directory
[service_ope.py-19] [INFO] [1729093691.676688178] [Ope_server]: Segmentation model: 20k_longlong
[service_ope.py-19] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:135: UserWarning: Using 'backbone_name' as positional parameter(s) is deprecated since 0.13 and may be removed in the future. Please use keyword parameter(s) instead.
[service_ope.py-19]   warnings.warn(
[service_ope.py-19] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:208: UserWarning: The parameter 'pretrained' is deprecated since 0.13 and may be removed in the future, please use 'weights' instead.
[service_ope.py-19]   warnings.warn(
[service_ope.py-19] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:223: UserWarning: Arguments other than a weight enum or `None` for 'weights' are deprecated since 0.13 and may be removed in the future. The current behavior is equivalent to passing `weights=None`.
[service_ope.py-19]   warnings.warn(msg)
[servo_pose_tracking-9] [WARN] [1729093698.111416663] [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.
[servo_pose_tracking-9] [INFO] [1729093698.136437638] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.planning_frame: base_link
[servo_pose_tracking-9] [INFO] [1729093698.136915626] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.move_group_name: manipulator
[servo_pose_tracking-9] [INFO] [1729093698.137441405] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.publish_period: 0.004
[servo_pose_tracking-9] [INFO] [1729093698.137788750] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.windup_limit: 0.1
[servo_pose_tracking-9] [INFO] [1729093698.138173508] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093698.138194167] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093698.138682283] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093698.139201556] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093698.139822704] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093698.140692724] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093698.141472661] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093698.142080615] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093698.143192493] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093698.143941719] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093698.144714021] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_proportional_gain: 30
[servo_pose_tracking-9] [INFO] [1729093698.145271716] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_integral_gain: 1
[servo_pose_tracking-9] [INFO] [1729093698.145855172] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_derivative_gain: 4
[servo_pose_tracking-9] [INFO] [1729093698.167077276] [moveit_servo.pose_tracking_demo]: Servo status: No warnings
[servo_pose_tracking-9] [WARN] [1729093698.168983854] [moveit_servo.pose_tracking]: Target control rate was missed
[ros2-1] [INFO] [1729093698.237878310] [rosbag2_recorder]: Subscribed to topic '/forward_position_controller/commands'
[ros2-1] [INFO] [1729093698.240397189] [rosbag2_recorder]: Subscribed to topic '/pose_tracking_demo/collision_velocity_scale'
[ros2-1] [INFO] [1729093698.242775571] [rosbag2_recorder]: Subscribed to topic '/pose_tracking_demo/status'
[ros2-1] [INFO] [1729093698.245393362] [rosbag2_recorder]: Subscribed to topic '/pose_tracking_demo/delta_twist_cmds'
[servo_pose_tracking-9] [INFO] [1729093698.667357161] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48143, Angular error equal to: 0.27630
[servo_pose_tracking-9] [INFO] [1729093698.667385579] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09724, Z: 0.45171
[servo_pose_tracking-9] [INFO] [1729093698.667398529] [moveit_servo.pose_tracking_demo]: roll: 0.03739, pitch: 6.00938, yaw: 6.27494
[servo_pose_tracking-9] [INFO] [1729093699.167548651] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48146, Angular error equal to: 0.27636
[servo_pose_tracking-9] [INFO] [1729093699.167581299] [moveit_servo.pose_tracking_demo]: X: -0.13521, Y: 0.09725, Z: 0.45174
[servo_pose_tracking-9] [INFO] [1729093699.167590427] [moveit_servo.pose_tracking_demo]: roll: 0.03736, pitch: 6.00931, yaw: 6.27495
[servo_pose_tracking-9] [INFO] [1729093699.667700524] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48145, Angular error equal to: 0.27634
[servo_pose_tracking-9] [INFO] [1729093699.667728757] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09726, Z: 0.45172
[servo_pose_tracking-9] [INFO] [1729093699.667737357] [moveit_servo.pose_tracking_demo]: roll: 0.03737, pitch: 6.00934, yaw: 6.27499
[servo_pose_tracking-9] [WARN] [1729093699.813305693] [moveit_servo.pose_tracking]: Target control rate was missed
[servo_pose_tracking-9] [INFO] [1729093700.167866261] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48146, Angular error equal to: 0.27635
[servo_pose_tracking-9] [INFO] [1729093700.167900763] [moveit_servo.pose_tracking_demo]: X: -0.13521, Y: 0.09725, Z: 0.45173
[servo_pose_tracking-9] [INFO] [1729093700.167914037] [moveit_servo.pose_tracking_demo]: roll: 0.03740, pitch: 6.00933, yaw: 6.27493
[servo_pose_tracking-9] [INFO] [1729093700.668031213] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48143, Angular error equal to: 0.27633
[servo_pose_tracking-9] [INFO] [1729093700.668061447] [moveit_servo.pose_tracking_demo]: X: -0.13519, Y: 0.09724, Z: 0.45170
[servo_pose_tracking-9] [INFO] [1729093700.668071491] [moveit_servo.pose_tracking_demo]: roll: 0.03734, pitch: 6.00935, yaw: 6.27498
[servo_pose_tracking-9] [INFO] [1729093700.668086747] [moveit_servo.pose_tracking_demo]: ERROR: The error is not decreasing, stopping the motion
[servo_pose_tracking-9] [INFO] [1729093700.668157244] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48143, Angular error equal to: 0.27633
[servo_pose_tracking-9] [INFO] [1729093700.668167888] [moveit_servo.pose_tracking_demo]: X: -0.13519, Y: 0.09724, Z: 0.45170
[servo_pose_tracking-9] [INFO] [1729093700.668175676] [moveit_servo.pose_tracking_demo]: roll: 0.03734, pitch: 6.00935, yaw: 6.27498
[servo_pose_tracking-9] [INFO] [1729093700.668184792] [moveit_servo.pose_tracking_demo]: Done!
[servo_pose_tracking-9] [INFO] [1729093700.669051881] [moveit_servo.pose_tracking]: Halting servo motion, a stop was requested.
[servo_pose_tracking-9] [INFO] [1729093700.669121805] [moveit_servo.pose_tracking_demo]: Pose tracker exited with status: Stop requested
[servo_pose_tracking-9] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
[servo_pose_tracking-9]          at line 127 in /opt/conda/build_artifacts/ros-humble-class-loader-0_1707311607763/work/ros-humble-class-loader/src/work/src/class_loader.cpp
[service_ope.py-19] [INFO] [1729093701.082690917] [Ope_server]: Done with M2P initialization.
[ur_ros2_control_node-6] [INFO] [1729093710.636325222] [UR_Client_Library:]: Robot requested program
[ur_ros2_control_node-6] [INFO] [1729093710.636388765] [UR_Client_Library:]: Sent program to robot
[ur_ros2_control_node-6] [INFO] [1729093712.864952719] [UR_Client_Library:]: Robot connected to reverse interface. Ready to receive control commands.
[servo_pose_tracking-9] [INFO] [1729093717.444033662] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.planning_frame: base_link
[servo_pose_tracking-9] [INFO] [1729093717.444692607] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.move_group_name: manipulator
[servo_pose_tracking-9] [INFO] [1729093717.445247459] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.publish_period: 0.004
[servo_pose_tracking-9] [INFO] [1729093717.445493618] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.windup_limit: 0.1
[servo_pose_tracking-9] [INFO] [1729093717.445927872] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093717.445952016] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093717.446287807] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093717.446707273] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_proportional_gain: 40
[servo_pose_tracking-9] [INFO] [1729093717.447007371] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093717.447366543] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093717.447719869] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_integral_gain: 0.05
[servo_pose_tracking-9] [INFO] [1729093717.448014705] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.x_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093717.448462127] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.y_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093717.448900168] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.z_derivative_gain: 3
[servo_pose_tracking-9] [INFO] [1729093717.449257044] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_proportional_gain: 30
[servo_pose_tracking-9] [INFO] [1729093717.449923360] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_integral_gain: 1
[servo_pose_tracking-9] [INFO] [1729093717.450528791] [moveit_servo.pose_tracking]: Found parameter - moveit_servo.angular_derivative_gain: 4
[servo_pose_tracking-9] [WARN] [1729093717.469922799] [moveit_servo.pose_tracking]: Target control rate was missed
[servo_pose_tracking-9] [INFO] [1729093717.470772681] [moveit_servo.pose_tracking_demo]: Servo status: No warnings
[servo_pose_tracking-9] [INFO] [1729093717.968887657] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48145, Angular error equal to: 0.27633
[servo_pose_tracking-9] [INFO] [1729093717.968914483] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09726, Z: 0.45172
[servo_pose_tracking-9] [INFO] [1729093717.968924776] [moveit_servo.pose_tracking_demo]: roll: 0.03740, pitch: 6.00935, yaw: 6.27497
[servo_pose_tracking-9] [INFO] [1729093718.469042171] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48147, Angular error equal to: 0.27639
[servo_pose_tracking-9] [INFO] [1729093718.469075298] [moveit_servo.pose_tracking_demo]: X: -0.13518, Y: 0.09726, Z: 0.45175
[servo_pose_tracking-9] [INFO] [1729093718.469085812] [moveit_servo.pose_tracking_demo]: roll: 0.03740, pitch: 6.00929, yaw: 6.27499
[servo_pose_tracking-9] [INFO] [1729093718.969210803] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48146, Angular error equal to: 0.27638
[servo_pose_tracking-9] [INFO] [1729093718.969239251] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09725, Z: 0.45174
[servo_pose_tracking-9] [INFO] [1729093718.969251940] [moveit_servo.pose_tracking_demo]: roll: 0.03735, pitch: 6.00930, yaw: 6.27496
[servo_pose_tracking-9] [INFO] [1729093719.469381288] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48146, Angular error equal to: 0.27638
[servo_pose_tracking-9] [INFO] [1729093719.469418142] [moveit_servo.pose_tracking_demo]: X: -0.13519, Y: 0.09726, Z: 0.45174
[servo_pose_tracking-9] [INFO] [1729093719.469431756] [moveit_servo.pose_tracking_demo]: roll: 0.03740, pitch: 6.00930, yaw: 6.27499
[servo_pose_tracking-9] [INFO] [1729093719.969554549] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48143, Angular error equal to: 0.27633
[servo_pose_tracking-9] [INFO] [1729093719.969585622] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09725, Z: 0.45170
[servo_pose_tracking-9] [INFO] [1729093719.969598546] [moveit_servo.pose_tracking_demo]: roll: 0.03737, pitch: 6.00935, yaw: 6.27499
[servo_pose_tracking-9] [INFO] [1729093719.969618374] [moveit_servo.pose_tracking_demo]: ERROR: The error is not decreasing, stopping the motion
[servo_pose_tracking-9] [INFO] [1729093719.969701919] [moveit_servo.pose_tracking_demo]: Linear Error equal to: 0.48143, Angular error equal to: 0.27633
[servo_pose_tracking-9] [INFO] [1729093719.969714696] [moveit_servo.pose_tracking_demo]: X: -0.13520, Y: 0.09725, Z: 0.45170
[servo_pose_tracking-9] [INFO] [1729093719.969725245] [moveit_servo.pose_tracking_demo]: roll: 0.03737, pitch: 6.00935, yaw: 6.27499
[servo_pose_tracking-9] [INFO] [1729093719.969737360] [moveit_servo.pose_tracking_demo]: Done!
Logging output robot doesn’t spawn 
[INFO] [launch]: All log files can be found below /home/icub/.ros/log/2024-10-16-12-17-40-885149-iiticublap158u-1120188
[INFO] [launch]: Default logging verbosity is set to INFO
Using calibrated kinematics file
[INFO] [dashboard_client-2]: process started with pid [1120194]
Launching your Real Robot config in MoveIt
WARNING:root:Cannot infer URDF from `/home/icub/code/project-jl2-camozzi/jl2_workspace/install/ur5e_moveit/share/ur5e_moveit`. -- using config/jl2.urdf
WARNING:root:Cannot infer SRDF from `/home/icub/code/project-jl2-camozzi/jl2_workspace/install/ur5e_moveit/share/ur5e_moveit`. -- using config/jl2.srdf
[INFO] [ros2-1]: process started with pid [1120192]
[INFO] [urscript_interface-3]: process started with pid [1120196]
[INFO] [robot_state_publisher-4]: process started with pid [1120198]
[INFO] [controller_stopper_node-5]: process started with pid [1120200]
Launching real robot
UseCase1: OPE algorithm to be launched: m2p
M2P will estimate the pose of objects with ID: 50-6512-1458
[INFO] [move_group-6]: process started with pid [1120252]
[INFO] [rviz2-7]: process started with pid [1120254]
[INFO] [servo_pose_tracking-8]: process started with pid [1120256]
[INFO] [srv_Camera.py-9]: process started with pid [1120290]
[INFO] [srv_GraspPlanner.py-10]: process started with pid [1120292]
[INFO] [srv_GripperReal.py-11]: process started with pid [1120294]
[INFO] [service_ope.py-12]: process started with pid [1120296]
[ros2-1] stdin is not a terminal device. Keyboard handling disabled.[INFO] [1729073862.051993457] [rosbag2_recorder]: Press SPACE for pausing/resuming
[ros2-1] [INFO] [1729073862.053586028] [rosbag2_storage]: Opened database '/media/icub/data/local_data/jl2_workspace/bag/bag_0.db3' for READ_WRITE.
[ros2-1] [INFO] [1729073862.054066670] [rosbag2_recorder]: Listening for topics...
[ros2-1] [INFO] [1729073862.054074403] [rosbag2_recorder]: Event publisher thread: Starting
[ros2-1] [INFO] [1729073862.056730027] [rosbag2_recorder]: Subscribed to topic '/rosout'
[ros2-1] [INFO] [1729073862.058564361] [rosbag2_recorder]: Subscribed to topic '/parameter_events'
[ros2-1] [INFO] [1729073862.059127371] [rosbag2_recorder]: Subscribed to topic '/events/write_split'
[ros2-1] [INFO] [1729073862.059200685] [rosbag2_recorder]: Recording...
[ros2-1] [INFO] [1729073862.161702113] [rosbag2_recorder]: Subscribed to topic '/robot_description'
[ros2-1] [INFO] [1729073862.162736923] [rosbag2_recorder]: Subscribed to topic '/tf_static'
[ros2-1] [INFO] [1729073862.163321296] [rosbag2_recorder]: Subscribed to topic '/tf'
[robot_state_publisher-4] [INFO] [1729073861.876217111] [robot_state_publisher]: got segment base
[robot_state_publisher-4] [INFO] [1729073861.876311184] [robot_state_publisher]: got segment base_link
[robot_state_publisher-4] [INFO] [1729073861.876328974] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-4] [INFO] [1729073861.876341479] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-4] [INFO] [1729073861.876352613] [robot_state_publisher]: got segment camera_link_gazebo
[robot_state_publisher-4] [INFO] [1729073861.876366124] [robot_state_publisher]: got segment camera_link_optical
[robot_state_publisher-4] [INFO] [1729073861.876378542] [robot_state_publisher]: got segment end_effector
[robot_state_publisher-4] [INFO] [1729073861.876391035] [robot_state_publisher]: got segment finger_1
[robot_state_publisher-4] [INFO] [1729073861.876403587] [robot_state_publisher]: got segment finger_2
[robot_state_publisher-4] [INFO] [1729073861.876416067] [robot_state_publisher]: got segment flange
[robot_state_publisher-4] [INFO] [1729073861.876428537] [robot_state_publisher]: got segment flange_link
[robot_state_publisher-4] [INFO] [1729073861.876442793] [robot_state_publisher]: got segment flange_to_camera
[robot_state_publisher-4] [INFO] [1729073861.876453387] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-4] [INFO] [1729073861.876461348] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-4] [INFO] [1729073861.876471118] [robot_state_publisher]: got segment rack_1
[robot_state_publisher-4] [INFO] [1729073861.876481621] [robot_state_publisher]: got segment rack_2
[robot_state_publisher-4] [INFO] [1729073861.876490515] [robot_state_publisher]: got segment root_link
[robot_state_publisher-4] [INFO] [1729073861.876499777] [robot_state_publisher]: got segment shaft
[robot_state_publisher-4] [INFO] [1729073861.876510035] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-4] [INFO] [1729073861.876521049] [robot_state_publisher]: got segment tool0
[robot_state_publisher-4] [INFO] [1729073861.876529614] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-4] [INFO] [1729073861.876540095] [robot_state_publisher]: got segment world
[robot_state_publisher-4] [INFO] [1729073861.876550570] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-4] [INFO] [1729073861.876558818] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-4] [INFO] [1729073861.876568240] [robot_state_publisher]: got segment wrist_3_link
[dashboard_client-2] [INFO] [1729073861.867613678] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-2] 
[ros2-1] [INFO] [1729073862.265789816] [rosbag2_recorder]: Subscribed to topic '/AngleErrorEndEffector/Float32'
[ros2-1] [INFO] [1729073862.266622160] [rosbag2_recorder]: Subscribed to topic '/ErrorNormEndEffector/Float32'
[ros2-1] [INFO] [1729073862.267746684] [rosbag2_recorder]: Subscribed to topic '/TargetPoseEndEffector/TransformStamped'
[ros2-1] [INFO] [1729073862.268867166] [rosbag2_recorder]: Subscribed to topic '/target_pose'
[ros2-1] [INFO] [1729073862.272297246] [rosbag2_recorder]: Subscribed to topic '/planning_scene'
[ros2-1] [INFO] [1729073862.273202484] [rosbag2_recorder]: Subscribed to topic '/robot_description_semantic'
[controller_stopper_node-5] [INFO] [1729073861.870585666] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[move_group-6] [INFO] [1729073862.239818216] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00779627 seconds
[move_group-6] [INFO] [1729073862.239852611] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[move_group-6] [WARN] [1729073862.249062208] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[move_group-6] [WARN] [1729073862.256819261] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[servo_pose_tracking-8] [INFO] [1729073862.217848471] [moveit_servo.pose_tracking_demo]: Server Created!
[servo_pose_tracking-8] [INFO] [1729073862.231213056] [MoveItServoPoseTracker]: lin_tol_val: 0.001500
[servo_pose_tracking-8] [INFO] [1729073862.231251745] [MoveItServoPoseTracker]: rot_tol: 0.050000
[servo_pose_tracking-8] [INFO] [1729073862.231265926] [MoveItServoPoseTracker]: timeout: 20.000000
[servo_pose_tracking-8] [INFO] [1729073862.231283279] [MoveItServoPoseTracker]: tracker_lin_tol_val: 0.000500
[servo_pose_tracking-8] [INFO] [1729073862.231297074] [MoveItServoPoseTracker]: tracker_rot_tol: 0.030000
[servo_pose_tracking-8] [INFO] [1729073862.231310664] [MoveItServoPoseTracker]: tracker_timeout: 15.000000
[servo_pose_tracking-8] [INFO] [1729073862.243767205] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00598342 seconds
[servo_pose_tracking-8] [INFO] [1729073862.243791277] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[servo_pose_tracking-8] [WARN] [1729073862.252684130] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[servo_pose_tracking-8] [WARN] [1729073862.260284579] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[move_group-6] [INFO] [1729073862.416758549] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-6] [INFO] [1729073862.416907016] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-6] [INFO] [1729073862.417321856] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-6] [INFO] [1729073862.417888018] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-6] [INFO] [1729073862.417905384] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-6] [INFO] [1729073862.419214374] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-6] [INFO] [1729073862.419237121] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-6] [INFO] [1729073862.419766372] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-6] [INFO] [1729073862.420323466] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-6] [WARN] [1729073862.420692426] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-6] [ERROR] [1729073862.420715443] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[servo_pose_tracking-8] [WARN] [1729073862.423522463] [moveit_servo.servo_parameters]: Parameter 'self_collision_proximity_threshold' should probably be less than or equal to 'scene_collision_proximity_threshold'. Check yaml file.
[move_group-6] [INFO] [1729073862.436446356] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-6] [INFO] [1729073862.452657734] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-6] [INFO] [1729073862.455141985] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-6] [INFO] [1729073862.455161498] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-6] [INFO] [1729073862.455165973] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-6] [INFO] [1729073862.455179667] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-6] [INFO] [1729073862.455192393] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-6] [INFO] [1729073862.455196796] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-6] [INFO] [1729073862.455207147] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-6] [INFO] [1729073862.455211641] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-6] [INFO] [1729073862.455215293] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-6] [INFO] [1729073862.455224772] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-6] [INFO] [1729073862.455228829] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-6] [INFO] [1729073862.455232065] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-6] [INFO] [1729073862.455259821] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-6] [INFO] [1729073862.455263615] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-6] [INFO] [1729073862.456780895] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-6] [INFO] [1729073862.459237277] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-6] [INFO] [1729073862.468629467] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-6] [INFO] [1729073862.468651394] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-6] [INFO] [1729073862.470086228] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-6] [INFO] [1729073862.470105850] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-6] [INFO] [1729073862.471101506] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-6] [INFO] [1729073862.471117946] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-6] [INFO] [1729073862.472010572] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-6] [INFO] [1729073862.472024895] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[ros2-1] [INFO] [1729073862.484946205] [rosbag2_recorder]: Subscribed to topic '/display_planned_path'
[ros2-1] [INFO] [1729073862.500403967] [rosbag2_recorder]: Subscribed to topic '/display_contacts'
[ros2-1] [INFO] [1729073862.511659139] [rosbag2_recorder]: Subscribed to topic '/monitored_planning_scene'
[move_group-6] [INFO] [1729073862.524046670] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller
[move_group-6] [INFO] [1729073862.527526240] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller
[move_group-6] [INFO] [1729073862.527665563] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-6] [INFO] [1729073862.527702245] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-6] [INFO] [1729073862.528194838] [moveit_ros.trajectory_execution_manager]: Trajectory execution is not managing controllers
[move_group-6] [INFO] [1729073862.528211882] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-6] [INFO] [1729073862.555555500] [move_group.move_group]: 
[move_group-6] 
[move_group-6] ********************************************************
[move_group-6] * MoveGroup using: 
[move_group-6] *     - ApplyPlanningSceneService
[move_group-6] *     - ClearOctomapService
[move_group-6] *     - CartesianPathService
[move_group-6] *     - ExecuteTrajectoryAction
[move_group-6] *     - GetPlanningSceneService
[move_group-6] *     - KinematicsService
[move_group-6] *     - MoveAction
[move_group-6] *     - MotionPlanService
[move_group-6] *     - QueryPlannersService
[move_group-6] *     - StateValidationService
[move_group-6] ********************************************************
[move_group-6] 
[move_group-6] [INFO] [1729073862.555600657] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-6] [INFO] [1729073862.555609340] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-6] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-6] Loading 'move_group/ClearOctomapService'...
[move_group-6] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-6] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-6] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-6] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-6] Loading 'move_group/MoveGroupMoveAction'...
[move_group-6] Loading 'move_group/MoveGroupPlanService'...
[move_group-6] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-6] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-6] 
[move_group-6] You can start planning now!
[move_group-6] 
[ros2-1] [WARN] [1729073862.613062973] [ROSBAG2_TRANSPORT]: Hidden topics are not recorded. Enable them with --include-hidden-topics
[ros2-1] [INFO] [1729073862.616527259] [rosbag2_recorder]: Subscribed to topic '/motion_plan_request'
[rviz2-7] [INFO] [1729073862.636290863] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-7] [INFO] [1729073862.636340436] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-7] [INFO] [1729073862.664971382] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-7] 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-7]          at line 253 in /home/icub/miniforge3/envs/m2p/include/class_loader/class_loader/class_loader_core.hpp
[ros2-1] [INFO] [1729073862.926224093] [rosbag2_recorder]: Subscribed to topic '/planning_scene_world'
[srv_Camera.py-9] [INFO] [1729073863.137341394] [camera_server]: Simulated Gazebo: False
[ros2-1] [INFO] [1729073863.234860877] [rosbag2_recorder]: Subscribed to topic '/camera/rgb_image'
[ros2-1] [INFO] [1729073863.235704050] [rosbag2_recorder]: Subscribed to topic '/camera/depth_image'
[srv_GraspPlanner.py-10] [INFO] [1729073863.363184042] [Grasp_service]: Waiting for the transformation between camera_link_optical and end_effector...
[srv_GraspPlanner.py-10] [INFO] [1729073863.364893202] [Grasp_service]: Got the transformation between camera_link_optical and end_effector!
[INFO] [ur_ros2_control_node-13]: process started with pid [1120374]
[ur_ros2_control_node-13] [WARN] [1729073865.203208989] [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.
[ur_ros2_control_node-13] text not specified in the tf_prefix tag
[ur_ros2_control_node-13] [INFO] [1729073865.204186379] [resource_manager]: Loading hardware 'ur' 
[ur_ros2_control_node-13] [INFO] [1729073865.206470850] [resource_manager]: Initialize hardware 'ur' 
[ur_ros2_control_node-13] [INFO] [1729073865.206569295] [resource_manager]: Successful initialization of hardware 'ur'
[ur_ros2_control_node-13] [INFO] [1729073865.206860384] [resource_manager]: 'configure' hardware 'ur' 
[ur_ros2_control_node-13] [INFO] [1729073865.206874885] [resource_manager]: Successful 'configure' of hardware 'ur'
[ur_ros2_control_node-13] [INFO] [1729073865.206919558] [resource_manager]: 'activate' hardware 'ur' 
[ur_ros2_control_node-13] [INFO] [1729073865.206928637] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-13] [INFO] [1729073865.206944887] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-13] [WARN] [1729073865.210044729] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ros2-1] [INFO] [1729073865.305482916] [rosbag2_recorder]: Subscribed to topic '/camera/pose_image'
[ur_ros2_control_node-13] [INFO] [1729073865.702382581] [UR_Client_Library:]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-13] [INFO] [1729073865.703713469] [UR_Client_Library:]: Setting up RTDE communication with frequency 500.000000
[service_ope.py-12] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:500: UserWarning: The value of the smallest subnormal for <class 'numpy.float64'> type is zero.
[service_ope.py-12]   setattr(self, word, getattr(machar, word).flat[0])
[service_ope.py-12] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:89: UserWarning: The value of the smallest subnormal for <class 'numpy.float64'> type is zero.
[service_ope.py-12]   return self._float_to_str(self.smallest_subnormal)
[rviz2-7] [ERROR] [1729073865.840257686] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-7] [INFO] [1729073865.853391279] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-7] [WARN] [1729073865.876811028] [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.
[service_ope.py-12] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:500: UserWarning: The value of the smallest subnormal for <class 'numpy.float32'> type is zero.
[service_ope.py-12]   setattr(self, word, getattr(machar, word).flat[0])
[service_ope.py-12] /home/icub/blender/blender-3.5.1-linux-x64/3.5/python/lib/python3.10/site-packages/numpy/core/getlimits.py:89: UserWarning: The value of the smallest subnormal for <class 'numpy.float32'> type is zero.
[service_ope.py-12]   return self._float_to_str(self.smallest_subnormal)
[rviz2-7] [INFO] [1729073865.921527676] [rviz2]: Stereo is NOT SUPPORTED
[ros2-1] [INFO] [1729073865.932968750] [rosbag2_recorder]: Subscribed to topic '/goal_pose'
[ros2-1] [INFO] [1729073865.934122828] [rosbag2_recorder]: Subscribed to topic '/initialpose'
[ros2-1] [INFO] [1729073865.935337593] [rosbag2_recorder]: Subscribed to topic '/rviz_visual_tools_gui'
[ros2-1] [INFO] [1729073865.936602372] [rosbag2_recorder]: Subscribed to topic '/clicked_point'
[rviz2-7] [INFO] [1729073865.941897532] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-7] [INFO] [1729073865.965750685] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-7] [INFO] [1729073866.239534584] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00453958 seconds
[rviz2-7] [INFO] [1729073866.239558035] [moveit_robot_model.robot_model]: Loading robot model 'jl2'...
[rviz2-7] [WARN] [1729073866.245138341] [moveit_robot_model.robot_model]: Link camera_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[rviz2-7] [WARN] [1729073866.250044929] [moveit_robot_model.robot_model]: Link end_effector has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[rviz2-7] [ERROR] [1729073866.264973426] [moveit_background_processing.background_processing]: Exception caught while processing action 'loadRobotModel': parameter 'robot_description_planning.joint_limits.shoulder_pan_joint.max_position' has invalid type: Wrong parameter type, parameter {robot_description_planning.joint_limits.shoulder_pan_joint.max_position} is of type {double}, setting it to {string} is not allowed.
[ur_ros2_control_node-13] [WARN] [1729073866.725746100] [UR_Client_Library:]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-13] [WARN] [1729073866.725766318] [UR_Client_Library:]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-13] [INFO] [1729073866.725773753] [URPositionHardwareInterface]: Calibration checksum: 'calib_11453222953044254961'.
[service_ope.py-12] => Templates are already in the right dir!
[service_ope.py-12] Blender 3.5.1 (hash e1ccd9d4a1d3 built 2023-04-24 23:31:15)
[service_ope.py-12] Read prefs: /home/icub/.config/blender/3.5/config/userpref.blend
[service_ope.py-12] 
[service_ope.py-12] Blender quit
[service_ope.py-12] Warning: Changed install path from /home_local/icub... to /home/icub..., there is no /home_local/ on this machine.
[service_ope.py-12] Using blender in /home/icub/blender/blender-3.5.1-linux-x64
[service_ope.py-12] Using temporary directory: /dev/shm/blender_proc_1521c698692b4f9ba3824f8e5b1cd68c
[service_ope.py-12] Cleaning temporary directory
[service_ope.py-12] [INFO] [1729073867.482983088] [Ope_server]: Segmentation model: 20k_longlong
[service_ope.py-12] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:135: UserWarning: Using 'backbone_name' as positional parameter(s) is deprecated since 0.13 and may be removed in the future. Please use keyword parameter(s) instead.
[service_ope.py-12]   warnings.warn(
[service_ope.py-12] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:208: UserWarning: The parameter 'pretrained' is deprecated since 0.13 and may be removed in the future, please use 'weights' instead.
[service_ope.py-12]   warnings.warn(
[service_ope.py-12] /home/icub/miniforge3/envs/m2p/lib/python3.11/site-packages/torchvision/models/_utils.py:223: UserWarning: Arguments other than a weight enum or `None` for 'weights' are deprecated since 0.13 and may be removed in the future. The current behavior is equivalent to passing `weights=None`.
[service_ope.py-12]   warnings.warn(msg)
[ur_ros2_control_node-13] [INFO] [1729073867.766257629] [URPositionHardwareInterface]: Calibration checked successfully.
[ur_ros2_control_node-13] [INFO] [1729073867.766308022] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-13] [INFO] [1729073867.766323070] [resource_manager]: Successful 'activate' of hardware 'ur'
[controller_stopper_node-5] [INFO] [1729073867.769180695] [Controller stopper]: Service available
[controller_stopper_node-5] [INFO] [1729073867.769397115] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-5] [INFO] [1729073867.769475144] [Controller stopper]: Service available
[ur_ros2_control_node-13] [WARN] [1729073867.770688710] [controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-13] [WARN] [1729073867.770907529] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[servo_pose_tracking-8] [INFO] [1729073872.424742753] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[servo_pose_tracking-8] [INFO] [1729073872.426057726] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[servo_pose_tracking-8] [INFO] [1729073872.426089119] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[servo_pose_tracking-8] [INFO] [1729073872.426620776] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[servo_pose_tracking-8] [INFO] [1729073872.427189240] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[servo_pose_tracking-8] [INFO] [1729073872.427702050] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[servo_pose_tracking-8] [INFO] [1729073872.428331358] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[servo_pose_tracking-8] [INFO] [1729073872.429656398] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[INFO] [spawner-14]: process started with pid [1120493]
[INFO] [spawner-15]: process started with pid [1120495]
[INFO] [spawner-16]: process started with pid [1120497]
[INFO] [spawner-17]: process started with pid [1120499]
[INFO] [spawner-18]: process started with pid [1120501]
[INFO] [spawner-19]: process started with pid [1120503]
[ur_ros2_control_node-13] [INFO] [1729073874.259331373] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-13] [INFO] [1729073874.291078869] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: 
[service_ope.py-12] [INFO] [1729073874.301998738] [Ope_server]: Done with M2P initialization.
[ros2-1] [INFO] [1729073874.345004383] [rosbag2_recorder]: Subscribed to topic '/speed_scaling_state_broadcaster/transition_event'
[spawner-17] [INFO] [1729073874.349275886] [spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-13] [INFO] [1729073874.350894971] [controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-13] [INFO] [1729073874.351008967] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[spawner-17] [INFO] [1729073874.358702593] [spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[ur_ros2_control_node-13] [INFO] [1729073874.449142372] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[ros2-1] [INFO] [1729073874.454623787] [rosbag2_recorder]: Subscribed to topic '/speed_scaling_state_broadcaster/speed_scaling'
[spawner-18] [INFO] [1729073874.464724013] [spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-13] [INFO] [1729073874.466384302] [controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[spawner-18] [INFO] [1729073874.480723580] [spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[ur_ros2_control_node-13] [INFO] [1729073874.483752514] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-13] [WARN] [1729073874.503782291] [scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[spawner-19] [INFO] [1729073874.506699456] [spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-13] [INFO] [1729073874.510274289] [controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-13] [INFO] [1729073874.510559407] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-13] [INFO] [1729073874.510606773] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-13] [INFO] [1729073874.510663926] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[ur_ros2_control_node-13] [INFO] [1729073874.511538413] [scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-13] [INFO] [1729073874.516372220] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[ur_ros2_control_node-13] [INFO] [1729073874.538803150] [controller_manager]: Loading controller 'io_and_status_controller'
[spawner-16] [INFO] [1729073874.553884273] [spawner_io_and_status_controller]: Loaded io_and_status_controller
[ur_ros2_control_node-13] [INFO] [1729073874.561120682] [controller_manager]: Configuring controller 'io_and_status_controller'
[ros2-1] [INFO] [1729073874.569114437] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/controller_state'
[ros2-1] [INFO] [1729073874.571574728] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/transition_event'
[ros2-1] [INFO] [1729073874.575014100] [rosbag2_recorder]: Subscribed to topic '/force_torque_sensor_broadcaster/wrench'
[ros2-1] [INFO] [1729073874.577613540] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/state'
[ros2-1] [INFO] [1729073874.579320892] [rosbag2_recorder]: Subscribed to topic '/force_torque_sensor_broadcaster/transition_event'
[ros2-1] [INFO] [1729073874.580579163] [rosbag2_recorder]: Subscribed to topic '/scaled_joint_trajectory_controller/transition_event'
[spawner-16] [INFO] [1729073874.670654624] [spawner_io_and_status_controller]: Configured and activated io_and_status_controller
[ros2-1] [INFO] [1729073874.696402005] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/robot_program_running'
[ros2-1] [INFO] [1729073874.700469069] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/tool_data'
[ros2-1] [INFO] [1729073874.703750849] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/safety_mode'
[ros2-1] [INFO] [1729073874.707584269] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/robot_mode'
[ros2-1] [INFO] [1729073874.709863215] [rosbag2_recorder]: Subscribed to topic '/io_and_status_controller/io_states'
[INFO] [spawner-19]: process has finished cleanly [pid 1120503]
[INFO] [spawner-17]: process has finished cleanly [pid 1120499]
[INFO] [spawner-16]: process has finished cleanly [pid 1120497]
[INFO] [spawner-18]: process has finished cleanly [pid 1120501]
```

</details>

@fmauch
Copy link
Collaborator

fmauch commented Oct 18, 2024 •
edited
Loading

This looks like you are running into the problems we tried to solve in ros-controls/ros2_control#1680 and ros-controls/ros2_control#1686.

Just to make sure: Do you have a up-to-date ros2_control installation?

To improve things, you could try to reduce the number of spawners, since the spawner now accepts a list of controllers.

In our launchfile we use two spawners for the fixed lists instead of the 6 that you are currently using (as we did in the past, as well):

Universal_Robots_ROS2_Driver/ur_robot_driver/launch/ur_control.launch.py

Lines 321 to 347 in 255355d

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",
"io_and_status_controller",
"speed_scaling_state_broadcaster",
"force_torque_sensor_broadcaster",
"ur_configuration_controller",
]
controllers_inactive = ["forward_position_controller"]
controller_spawners = [controller_spawner(controllers_active)] + [
controller_spawner(controllers_inactive, active=False)
]

Depending on which controller is missing, behavior will be different, hence the different faults you encounter.

@AleTarsi
Copy link
Author

AleTarsi commented Oct 21, 2024 •
edited
Loading

Just to make sure: Do you have a up-to-date ros2_control installation?

I am using a conda environment for my ros dependencies, using the robostack channel. For this reason I may not be able to have the latest version of all the packages.

Here is my mamba list with the versions of all my ros packages:

Mamba List 
# Name                    Version                   Build  Channel

ros-humble-ackermann-msgs 2.0.2           py311hb335429_6    robostack-staging
ros-humble-ackermann-steering-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-action-msgs    1.2.1           py311hb335429_6    robostack-staging
ros-humble-action-tutorials-cpp 0.20.3          py311hb335429_6    robostack-staging
ros-humble-action-tutorials-interfaces 0.20.3          py311hb335429_6    robostack-staging
ros-humble-action-tutorials-py 0.20.3          py311hb335429_6    robostack-staging
ros-humble-actionlib-msgs 4.2.3           py311hb335429_6    robostack-staging
ros-humble-admittance-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-ament-cmake    1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-auto 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-copyright 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-core 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-cppcheck 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-cpplint 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-definitions 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-dependencies 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-include-directories 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-interfaces 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-libraries 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-link-flags 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-export-targets 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-flake8 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-gen-version-h 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-gmock 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-gtest 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-include-directories 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-libraries 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-lint-cmake 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-pep257 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-pytest 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-python 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-ros 0.10.0          py311hb335429_6    robostack-staging
ros-humble-ament-cmake-target-dependencies 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-test 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-uncrustify 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cmake-version 1.3.7           py311hb335429_6    robostack-staging
ros-humble-ament-cmake-xmllint 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-copyright 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cppcheck 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-cpplint  0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-flake8   0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-index-cpp 1.4.0           py311hb335429_6    robostack-staging
ros-humble-ament-index-python 1.4.0           py311hb335429_6    robostack-staging
ros-humble-ament-lint     0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-lint-auto 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-lint-cmake 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-lint-common 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-package  0.14.0          py311hb335429_6    robostack-staging
ros-humble-ament-pep257   0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-uncrustify 0.12.10         py311hb335429_6    robostack-staging
ros-humble-ament-xmllint  0.12.10         py311hb335429_6    robostack-staging
ros-humble-angles         1.15.0          py311hb335429_6    robostack-staging
ros-humble-backward-ros   1.0.2           py311h4ff203a_6    robostack-staging
ros-humble-bicycle-steering-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-builtin-interfaces 1.2.1           py311hb335429_6    robostack-staging
ros-humble-camera-calibration-parsers 3.1.8           py311hb335429_6    robostack-staging
ros-humble-camera-info-manager 3.1.8           py311hb335429_6    robostack-staging
ros-humble-class-loader   2.2.0           py311hb303436_6    robostack-staging
ros-humble-common-interfaces 4.2.3           py311hb335429_6    robostack-staging
ros-humble-composition    0.20.3          py311hb335429_6    robostack-staging
ros-humble-composition-interfaces 1.2.1           py311hb335429_6    robostack-staging
ros-humble-console-bridge-vendor 1.4.1           py311hb303436_6    robostack-staging
ros-humble-control-msgs   4.4.0           py311hb335429_6    robostack-staging
ros-humble-control-toolbox 3.2.0           py311hb335429_6    robostack-staging
ros-humble-controller-interface 2.38.0          py311hb335429_6    robostack-staging
ros-humble-controller-manager 2.38.0          py311hb335429_6    robostack-staging
ros-humble-controller-manager-msgs 2.38.0          py311hb335429_6    robostack-staging
ros-humble-cv-bridge      3.2.1           py311h5fd4792_6    robostack-staging
ros-humble-cyclonedds     0.10.4          py311h8eb0f6d_6    robostack-staging
ros-humble-demo-nodes-cpp 0.20.3          py311hb335429_6    robostack-staging
ros-humble-demo-nodes-cpp-native 0.20.3          py311hb335429_6    robostack-staging
ros-humble-demo-nodes-py  0.20.3          py311hb335429_6    robostack-staging
ros-humble-depthimage-to-laserscan 2.5.1           py311h5fd4792_6    robostack-staging
ros-humble-desktop        0.10.0          py311hb335429_6    robostack-staging
ros-humble-diagnostic-msgs 4.2.3           py311hb335429_6    robostack-staging
ros-humble-diff-drive-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-domain-coordinator 0.10.0          py311hb335429_6    robostack-staging
ros-humble-dummy-map-server 0.20.3          py311hb335429_6    robostack-staging
ros-humble-dummy-robot-bringup 0.20.3          py311hb335429_6    robostack-staging
ros-humble-dummy-sensors  0.20.3          py311hb335429_6    robostack-staging
ros-humble-effort-controllers 2.32.0          py311hb335429_6    robostack-staging
ros-humble-eigen-stl-containers 1.0.0           py311hb335429_6    robostack-staging
ros-humble-eigen3-cmake-module 0.1.1           py311hb335429_6    robostack-staging
ros-humble-example-interfaces 0.9.3           py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-action-client 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-action-server 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-client 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-composition 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-publisher 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-service 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-subscriber 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-minimal-timer 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclcpp-multithreaded-executor 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-executors 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-action-client 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-action-server 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-client 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-publisher 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-service 0.15.1          py311hb335429_6    robostack-staging
ros-humble-examples-rclpy-minimal-subscriber 0.15.1          py311hb335429_6    robostack-staging
ros-humble-fastcdr        1.0.24          py311hb335429_6    robostack-staging
ros-humble-fastrtps       2.6.7           py311h70423f0_6    robostack-staging
ros-humble-fastrtps-cmake-module 2.2.2           py311hb335429_6    robostack-staging
ros-humble-filters        2.1.0           py311hb335429_6    robostack-staging
ros-humble-foonathan-memory-vendor 1.2.0           py311hb335429_6    robostack-staging
ros-humble-force-torque-sensor-broadcaster 2.32.0          py311hb335429_6    robostack-staging
ros-humble-forward-command-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-gazebo-dev     3.7.0           py311h281bc2f_6    robostack-staging
ros-humble-gazebo-msgs    3.7.0           py311hb335429_6    robostack-staging
ros-humble-gazebo-plugins 3.7.0           py311hb335429_6    robostack-staging
ros-humble-gazebo-ros     3.7.0           py311hb335429_6    robostack-staging
ros-humble-gazebo-ros2-control 0.4.6           py311hb335429_6    robostack-staging
ros-humble-generate-parameter-library 0.3.7           py311h5888c24_6    robostack-staging
ros-humble-generate-parameter-library-py 0.3.7           py311hb335429_6    robostack-staging
ros-humble-geometric-shapes 2.1.3           py311h391de45_6    robostack-staging
ros-humble-geometry-msgs  4.2.3           py311hb335429_6    robostack-staging
ros-humble-geometry2      0.25.5          py311hb335429_6    robostack-staging
ros-humble-gmock-vendor   1.10.9004       py311hb335429_6    robostack-staging
ros-humble-graph-msgs     0.2.0           py311hb335429_6    robostack-staging
ros-humble-gripper-controllers 2.32.0          py311hb335429_6    robostack-staging
ros-humble-gtest-vendor   1.10.9004       py311hb335429_6    robostack-staging
ros-humble-hardware-interface 2.38.0          py311hb335429_6    robostack-staging
ros-humble-iceoryx-binding-c 2.0.5           py311hb335429_6    robostack-staging
ros-humble-iceoryx-hoofs  2.0.5           py311hb335429_6    robostack-staging
ros-humble-iceoryx-posh   2.0.5           py311hb335429_6    robostack-staging
ros-humble-ignition-cmake2-vendor 0.0.2           py311hd50fb47_6    robostack-staging
ros-humble-ignition-math6-vendor 0.0.2           py311hb335429_6    robostack-staging
ros-humble-image-geometry 3.2.1           py311h5fd4792_6    robostack-staging
ros-humble-image-tools    0.20.3          py311h5fd4792_6    robostack-staging
ros-humble-image-transport 3.1.8           py311hb335429_6    robostack-staging
ros-humble-imu-sensor-broadcaster 2.32.0          py311hb335429_6    robostack-staging
ros-humble-interactive-markers 2.3.2           py311hb335429_6    robostack-staging
ros-humble-intra-process-demo 0.20.3          py311h5fd4792_6    robostack-staging
ros-humble-joint-limits   2.38.0          py311hb335429_6    robostack-staging
ros-humble-joint-state-broadcaster 2.32.0          py311hb335429_6    robostack-staging
ros-humble-joint-state-publisher 2.4.0           py311hb335429_6    robostack-staging
ros-humble-joint-state-publisher-gui 2.4.0           py311hb335429_6    robostack-staging
ros-humble-joint-trajectory-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-joy            3.3.0           py311hb335429_6    robostack-staging
ros-humble-kdl-parser     2.6.4           py311hb335429_6    robostack-staging
ros-humble-keyboard-handler 0.0.5           py311hb335429_6    robostack-staging
ros-humble-kinematics-interface 0.2.0           py311hb335429_6    robostack-staging
ros-humble-laser-geometry 2.4.0           py311hb335429_6    robostack-staging
ros-humble-launch         1.0.4           py311hb335429_6    robostack-staging
ros-humble-launch-param-builder 0.1.1           py311hb335429_6    robostack-staging
ros-humble-launch-ros     0.19.7          py311hb335429_6    robostack-staging
ros-humble-launch-testing 1.0.4           py311hb335429_6    robostack-staging
ros-humble-launch-testing-ament-cmake 1.0.4           py311hb335429_6    robostack-staging
ros-humble-launch-testing-ros 0.19.7          py311hb335429_6    robostack-staging
ros-humble-launch-xml     1.0.4           py311hb335429_6    robostack-staging
ros-humble-launch-yaml    1.0.4           py311hb335429_6    robostack-staging
ros-humble-libcurl-vendor 3.1.1           py311hfbeaf8d_6    robostack-staging
ros-humble-libstatistics-collector 1.3.1           py311hb335429_6    robostack-staging
ros-humble-libyaml-vendor 1.2.2           py311hb335429_6    robostack-staging
ros-humble-lifecycle      0.20.3          py311hb335429_6    robostack-staging
ros-humble-lifecycle-msgs 1.2.1           py311hb335429_6    robostack-staging
ros-humble-logging-demo   0.20.3          py311hb335429_6    robostack-staging
ros-humble-map-msgs       2.1.0           py311hb335429_6    robostack-staging
ros-humble-message-filters 4.3.3           py311hb335429_6    robostack-staging
ros-humble-moveit         2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-common  2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-configs-utils 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-core    2.5.5           py311h391de45_6    robostack-staging
ros-humble-moveit-kinematics 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-msgs    2.2.1           py311hb335429_6    robostack-staging
ros-humble-moveit-planners 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-planners-ompl 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-plugins 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros     2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-benchmarks 2.5.5           py311h141ba8b_6    robostack-staging
ros-humble-moveit-ros-move-group 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-occupancy-map-monitor 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-planning 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-planning-interface 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-robot-interaction 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-ros-visualization 2.5.5           py311h27406db_6    robostack-staging
ros-humble-moveit-ros-warehouse 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-servo   2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-setup-app-plugins 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-setup-assistant 2.5.5           py311h27406db_6    robostack-staging
ros-humble-moveit-setup-controllers 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-setup-core-plugins 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-setup-framework 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-setup-srdf-plugins 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-simple-controller-manager 2.5.5           py311hb335429_6    robostack-staging
ros-humble-moveit-visual-tools 4.1.0           py311hb335429_6    robostack-staging
ros-humble-nav-msgs       4.2.3           py311hb335429_6    robostack-staging
ros-humble-object-recognition-msgs 2.0.0           py311hb335429_6    robostack-staging
ros-humble-octomap-msgs   2.0.0           py311hb335429_6    robostack-staging
ros-humble-ompl           1.6.0           py311h284c4b5_6    robostack-staging
ros-humble-orocos-kdl-vendor 0.2.5           py311hb335429_6    robostack-staging
ros-humble-osrf-pycommon  2.0.2           py311hb335429_6    robostack-staging
ros-humble-parameter-traits 0.3.7           py311h5888c24_6    robostack-staging
ros-humble-pcl-conversions 2.4.0           py311hf85fee4_6    robostack-staging
ros-humble-pcl-msgs       1.0.0           py311hb335429_6    robostack-staging
ros-humble-pendulum-control 0.20.3          py311hb335429_6    robostack-staging
ros-humble-pendulum-msgs  0.20.3          py311hb335429_6    robostack-staging
ros-humble-pilz-industrial-motion-planner 2.5.5           py311h9ee2941_6    robostack-staging
ros-humble-pluginlib      5.1.0           py311hb335429_6    robostack-staging
ros-humble-position-controllers 2.32.0          py311hb335429_6    robostack-staging
ros-humble-pybind11-vendor 2.4.2           py311hb335429_6    robostack-staging
ros-humble-python-cmake-module 0.10.0          py311hb335429_6    robostack-staging
ros-humble-python-qt-binding 1.1.2           py311hf4f0b2d_6    robostack-staging
ros-humble-qt-dotgraph    2.2.3           py311hb335429_6    robostack-staging
ros-humble-qt-gui         2.2.3           py311hf4f0b2d_6    robostack-staging
ros-humble-qt-gui-cpp     2.2.3           py311h27406db_6    robostack-staging
ros-humble-qt-gui-py-common 2.2.3           py311hb335429_6    robostack-staging
ros-humble-quality-of-service-demo-cpp 0.20.3          py311hb335429_6    robostack-staging
ros-humble-quality-of-service-demo-py 0.20.3          py311hb335429_6    robostack-staging
ros-humble-random-numbers 2.0.1           py311hb335429_6    robostack-staging
ros-humble-range-sensor-broadcaster 2.32.0          py311hb335429_6    robostack-staging
ros-humble-rcl            5.3.7           py311hb335429_6    robostack-staging
ros-humble-rcl-action     5.3.7           py311hb335429_6    robostack-staging
ros-humble-rcl-interfaces 1.2.1           py311hb335429_6    robostack-staging
ros-humble-rcl-lifecycle  5.3.7           py311hb335429_6    robostack-staging
ros-humble-rcl-logging-interface 2.3.1           py311hb335429_6    robostack-staging
ros-humble-rcl-logging-spdlog 2.3.1           py311h9883907_6    robostack-staging
ros-humble-rcl-yaml-param-parser 5.3.7           py311hb335429_6    robostack-staging
ros-humble-rclcpp         16.0.8          py311hb335429_6    robostack-staging
ros-humble-rclcpp-action  16.0.8          py311hb335429_6    robostack-staging
ros-humble-rclcpp-components 16.0.8          py311hb335429_6    robostack-staging
ros-humble-rclcpp-lifecycle 16.0.8          py311hb335429_6    robostack-staging
ros-humble-rclpy          3.3.11          py311hb335429_6    robostack-staging
ros-humble-rcpputils      2.4.1           py311hb335429_6    robostack-staging
ros-humble-rcutils        5.1.4           py311hb335429_6    robostack-staging
ros-humble-realtime-tools 2.5.0           py311hb335429_6    robostack-staging
ros-humble-resource-retriever 3.1.1           py311hb335429_6    robostack-staging
ros-humble-rmw            6.1.1           py311hb335429_6    robostack-staging
ros-humble-rmw-connextdds 0.11.2          py311hb335429_6    robostack-staging
ros-humble-rmw-connextdds-common 0.11.2          py311hb335429_6    robostack-staging
ros-humble-rmw-cyclonedds-cpp 1.3.4           py311hb335429_6    robostack-staging
ros-humble-rmw-dds-common 1.6.0           py311hb335429_6    robostack-staging
ros-humble-rmw-fastrtps-cpp 6.2.6           py311hb335429_6    robostack-staging
ros-humble-rmw-fastrtps-dynamic-cpp 6.2.6           py311hb335429_6    robostack-staging
ros-humble-rmw-fastrtps-shared-cpp 6.2.6           py311hb335429_6    robostack-staging
ros-humble-rmw-implementation 2.8.2           py311hb335429_6    robostack-staging
ros-humble-rmw-implementation-cmake 6.1.1           py311hb335429_6    robostack-staging
ros-humble-robot-state-publisher 3.0.3           py311hb335429_6    robostack-staging
ros-humble-ros-base       0.10.0          py311hb335429_6    robostack-staging
ros-humble-ros-core       0.10.0          py311hb335429_6    robostack-staging
ros-humble-ros-environment 3.2.2           py311hb335429_6    robostack-staging
ros-humble-ros-workspace  1.0.2           py311hb335429_6    robostack-staging
ros-humble-ros2-control   2.38.0          py311hb335429_6    robostack-staging
ros-humble-ros2-control-test-assets 2.38.0          py311hb335429_6    robostack-staging
ros-humble-ros2-controllers 2.32.0          py311hb335429_6    robostack-staging
ros-humble-ros2-controllers-test-nodes 2.32.0          py311hb335429_6    robostack-staging
ros-humble-ros2action     0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2bag        0.15.9          py311hb335429_6    robostack-staging
ros-humble-ros2cli        0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2cli-common-extensions 0.1.1           py311hb335429_6    robostack-staging
ros-humble-ros2component  0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2controlcli 2.38.0          py311hb335429_6    robostack-staging
ros-humble-ros2doctor     0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2interface  0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2launch     0.19.7          py311hb335429_6    robostack-staging
ros-humble-ros2lifecycle  0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2multicast  0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2node       0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2param      0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2pkg        0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2run        0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2service    0.18.8          py311hb335429_6    robostack-staging
ros-humble-ros2topic      0.18.8          py311hb335429_6    robostack-staging
ros-humble-rosbag2        0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-compression 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-compression-zstd 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-cpp    0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-interfaces 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-py     0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-storage 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-storage-default-plugins 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosbag2-transport 0.15.9          py311hb335429_6    robostack-staging
ros-humble-rosgraph-msgs  1.2.1           py311hb335429_6    robostack-staging
ros-humble-rosidl-adapter 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-cli     3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-cmake   3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-default-generators 1.2.0           py311hb335429_6    robostack-staging
ros-humble-rosidl-default-runtime 1.2.0           py311hb335429_6    robostack-staging
ros-humble-rosidl-generator-c 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-generator-cpp 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-generator-py 0.14.4          py311hb335429_6    robostack-staging
ros-humble-rosidl-parser  3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-runtime-c 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-runtime-cpp 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-runtime-py 0.9.3           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-c 2.0.1           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-cpp 2.0.1           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-fastrtps-c 2.2.2           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-fastrtps-cpp 2.2.2           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-interface 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-introspection-c 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rosidl-typesupport-introspection-cpp 3.1.5           py311hb335429_6    robostack-staging
ros-humble-rpyutils       0.2.1           py311hb335429_6    robostack-staging
ros-humble-rqt-action     2.0.1           py311hb335429_6    robostack-staging
ros-humble-rqt-bag        1.1.4           py311hb335429_6    robostack-staging
ros-humble-rqt-bag-plugins 1.1.4           py311hb335429_6    robostack-staging
ros-humble-rqt-common-plugins 1.2.0           py311hb335429_6    robostack-staging
ros-humble-rqt-console    2.0.2           py311hb335429_6    robostack-staging
ros-humble-rqt-graph      1.3.0           py311hb335429_6    robostack-staging
ros-humble-rqt-gui        1.1.6           py311hb335429_6    robostack-staging
ros-humble-rqt-gui-cpp    1.1.6           py311h27406db_6    robostack-staging
ros-humble-rqt-gui-py     1.1.6           py311hb335429_6    robostack-staging
ros-humble-rqt-image-view 1.2.0           py311h27406db_6    robostack-staging
ros-humble-rqt-msg        1.2.0           py311hb335429_6    robostack-staging
ros-humble-rqt-plot       1.1.2           py311hb335429_6    robostack-staging
ros-humble-rqt-publisher  1.5.0           py311hb335429_6    robostack-staging
ros-humble-rqt-py-common  1.1.6           py311h27406db_6    robostack-staging
ros-humble-rqt-py-console 1.0.2           py311hb335429_6    robostack-staging
ros-humble-rqt-reconfigure 1.1.2           py311hb335429_6    robostack-staging
ros-humble-rqt-service-caller 1.0.5           py311hb335429_6    robostack-staging
ros-humble-rqt-shell      1.0.2           py311hb335429_6    robostack-staging
ros-humble-rqt-srv        1.0.3           py311hb335429_6    robostack-staging
ros-humble-rqt-topic      1.5.0           py311hb335429_6    robostack-staging
ros-humble-rsl            1.1.0           py311hba9e1f2_6    robostack-staging
ros-humble-rti-connext-dds-cmake-module 0.11.2          py311hb335429_6    robostack-staging
ros-humble-rttest         0.13.0          py311hb335429_6    robostack-staging
ros-humble-ruckig         0.9.2           py311hb335429_6    robostack-staging
ros-humble-rviz-assimp-vendor 11.2.10         py311h391de45_6    robostack-staging
ros-humble-rviz-common    11.2.10         py311h27406db_6    robostack-staging
ros-humble-rviz-default-plugins 11.2.10         py311h27406db_6    robostack-staging
ros-humble-rviz-ogre-vendor 11.2.10         py311hcb5fb3e_6    robostack-staging
ros-humble-rviz-rendering 11.2.10         py311hd4beeb7_6    robostack-staging
ros-humble-rviz-visual-tools 4.1.4           py311h27406db_6    robostack-staging
ros-humble-rviz2          11.2.10         py311h27406db_6    robostack-staging
ros-humble-sdl2-vendor    3.3.0           py311h5e8cc36_6    robostack-staging
ros-humble-sensor-msgs    4.2.3           py311hb335429_6    robostack-staging
ros-humble-sensor-msgs-py 4.2.3           py311hb335429_6    robostack-staging
ros-humble-shape-msgs     4.2.3           py311hb335429_6    robostack-staging
ros-humble-shared-queues-vendor 0.15.9          py311hb335429_6    robostack-staging
ros-humble-spdlog-vendor  1.3.1           py311h9883907_6    robostack-staging
ros-humble-sqlite3-vendor 0.15.9          py311he5a647e_6    robostack-staging
ros-humble-srdfdom        2.0.4           py311hb303436_6    robostack-staging
ros-humble-sros2          0.10.4          py311hb335429_6    robostack-staging
ros-humble-sros2-cmake    0.10.4          py311hb335429_6    robostack-staging
ros-humble-statistics-msgs 1.2.1           py311hb335429_6    robostack-staging
ros-humble-std-msgs       4.2.3           py311hb335429_6    robostack-staging
ros-humble-std-srvs       4.2.3           py311hb335429_6    robostack-staging
ros-humble-steering-controllers-library 2.32.0          py311hb335429_6    robostack-staging
ros-humble-stereo-msgs    4.2.3           py311hb335429_6    robostack-staging
ros-humble-tango-icons-vendor 0.1.1           py311hb335429_6    robostack-staging
ros-humble-tcb-span       1.0.2           py311hb335429_6    robostack-staging
ros-humble-teleop-twist-joy 2.4.5           py311hb335429_6    robostack-staging
ros-humble-teleop-twist-keyboard 2.3.2           py311hb335429_6    robostack-staging
ros-humble-tf2            0.25.5          py311hb303436_6    robostack-staging
ros-humble-tf2-bullet     0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-eigen      0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-eigen-kdl  0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-geometry-msgs 0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-kdl        0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-msgs       0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-py         0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-ros        0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-ros-py     0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-sensor-msgs 0.25.5          py311hb335429_6    robostack-staging
ros-humble-tf2-tools      0.25.5          py311hb335429_6    robostack-staging
ros-humble-tinyxml-vendor 0.8.3           py311hb335429_6    robostack-staging
ros-humble-tinyxml2-vendor 0.7.6           py311hfc76a15_6    robostack-staging
ros-humble-tl-expected    1.0.2           py311hb335429_6    robostack-staging
ros-humble-tlsf           0.7.0           py311hb335429_6    robostack-staging
ros-humble-tlsf-cpp       0.13.0          py311hb335429_6    robostack-staging
ros-humble-topic-monitor  0.20.3          py311hb335429_6    robostack-staging
ros-humble-tracetools     4.1.1           py311hb335429_6    robostack-staging
ros-humble-trajectory-msgs 4.2.3           py311hb335429_6    robostack-staging
ros-humble-transmission-interface 2.38.0          py311hb335429_6    robostack-staging
ros-humble-tricycle-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-tricycle-steering-controller 2.32.0          py311hb335429_6    robostack-staging
ros-humble-turtlesim      1.4.2           py311h27406db_6    robostack-staging
ros-humble-uncrustify-vendor 2.0.2           py311hb335429_6    robostack-staging
ros-humble-unique-identifier-msgs 2.2.1           py311hb335429_6    robostack-staging
ros-humble-ur             2.2.10          py311hb335429_6    robostack-staging
ros-humble-ur-calibration 2.2.10          py311hb335429_6    robostack-staging
ros-humble-ur-client-library 1.3.4           py311hb335429_6    robostack-staging
ros-humble-ur-controllers 2.2.10          py311hb335429_6    robostack-staging
ros-humble-ur-dashboard-msgs 2.2.10          py311hb335429_6    robostack-staging
ros-humble-ur-description 2.1.3           py311h0d0431f_6    robostack-staging
ros-humble-ur-moveit-config 2.2.10          py311hb335429_6    robostack-staging
ros-humble-ur-msgs        2.0.0           py311hb335429_6    robostack-staging
ros-humble-ur-robot-driver 2.2.10          py311hb335429_6    robostack-staging
ros-humble-urdf           2.6.0           py311hb335429_6    robostack-staging
ros-humble-urdf-parser-plugin 2.6.0           py311hb335429_6    robostack-staging
ros-humble-urdfdom        3.0.2           py311hb303436_6    robostack-staging
ros-humble-urdfdom-headers 1.0.6           py311hb335429_6    robostack-staging
ros-humble-urdfdom-py     1.2.1           py311hb335429_6    robostack-staging
ros-humble-velocity-controllers 2.32.0          py311hb335429_6    robostack-staging
ros-humble-visualization-msgs 4.2.3           py311hb335429_6    robostack-staging
ros-humble-warehouse-ros  2.0.4           py311h8eb0f6d_6    robostack-staging
ros-humble-warehouse-ros-sqlite 1.0.3           py311he5a647e_6    robostack-staging
ros-humble-xacro          2.0.8           py311hb335429_6    robostack-staging
ros-humble-yaml-cpp-vendor 8.0.2           py311hb335429_6    robostack-staging
ros-humble-zstd-vendor    0.15.9          py311hefdfea7_6    robostack-staging
ros2-distro-mutex         0.5.0                    humble    robostack-staging
rosdistro                 0.9.0           py311h38be061_1    conda-forge
rospkg                    1.5.1              pyhd8ed1ab_0    conda-forge

As you can see ros-humble-ros2-control == 2.38.0 , while the latest release should be 2.43.1

To improve things, you could try to reduce the number of spawners, since the spawner now accepts a list of controllers.

I have implemented your suggestions, I will now perform some tests and get back to you.

@fmauch
Copy link
Collaborator

fmauch commented Oct 21, 2024 •
edited
Loading

As you can see ros-humble-ros2-control == 2.38.0 , the latest release should be 2.43.1

This fix is only available from version 2.43 on.

Reducing the number of spawners might still improve things, but you will probably still run into this from time to time unless upgrading.

@AleTarsi
Copy link
Author

AleTarsi commented Oct 28, 2024 •
edited
Loading

Your suggestion helped, at least, reducing the frequency of the problem. So far I haven't encountered any troubles related to the controllers not spawning. Thanks!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@fmauch @AleTarsi