search

UniversalRobots / Universal_Robots_ROS2_Driver

Universal Robots ROS2 driver supporting CB3 and e-Series
BSD 3-Clause "New" or "Revised" License
426 stars 220 forks source link

[ur_robot_driver] ur_ros2_control_node crashes when loading robot_description from a topic #932

Closed mrjogo closed 8 months ago

mrjogo commented 8 months ago

Affected ROS2 Driver version(s)

2.2.10-1jammy.20240126.080209

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.13.0

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

Passing the robot_description to ur_ros2_control_node as a topic instead of a parameter causes it to crash

Issue details

To have only one source of truth / DRY, the robot_description should be read off of /robot_description published by robot_state_publisher, not passed as a param. Doing so, however, causes a crash. Could be related to https://github.com/ros-controls/ros2_control/issues/1262.

Steps to Reproduce

Here is a minimal launchfile that reproduces the failure:

# reproduce.launch.py
# ros2 launch my_pkg reproduce.launch.py use_topic:=true
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.actions import SetLaunchConfiguration, DeclareLaunchArgument
from launch.substitutions import (
    LaunchConfiguration,
    Command,
    FindExecutable,
    PathJoinSubstitution,
)
from launch.conditions import IfCondition, UnlessCondition

def generate_launch_description():
    # Vars setup
    set_tf_prefix = SetLaunchConfiguration("tf_prefix", "")
    use_topic = LaunchConfiguration("use_topic")
    declare_use_topic = DeclareLaunchArgument(
        "use_topic",
        default_value="true",
        description="Use topic to pass robot_description to ur_ros2_control_node",
    )

    # Robot description
    script_filename = PathJoinSubstitution(
        [
            FindPackageShare("ur_client_library"),
            "resources",
            "external_control.urscript",
        ]
    )
    input_recipe_filename = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
    )
    output_recipe_filename = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
    )
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("ur_description"), "urdf", "ur.urdf.xacro"]
            ),
            " ",
            "name:=ur",
            " ",
            "ur_type:=ur5e",
            " ",
            "robot_ip:=192.168.3.19",
            " ",
            "script_filename:=",
            script_filename,
            " ",
            "input_recipe_filename:=",
            input_recipe_filename,
            " ",
            "output_recipe_filename:=",
            output_recipe_filename,
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    # Robot state publisher node
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    # UR control node
    update_rate_config_file = PathJoinSubstitution(
        [
            FindPackageShare("ur_robot_driver"),
            "config",
            "ur5e_update_rate.yaml",
        ]
    )
    initial_joint_controllers = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "config", "ur_controllers.yaml"]
    )

    # Get robot_description from topic
    ur_control_node_topic = Node(
        package="ur_robot_driver",
        executable="ur_ros2_control_node",
        parameters=[
            update_rate_config_file,
            ParameterFile(initial_joint_controllers, allow_substs=True),
        ],
        remappings=[("~/robot_description", "robot_description")],
        output="screen",
        condition=IfCondition(use_topic),
    )

    # Get robot_description from parameter
    ur_control_node_parameter = Node(
        package="ur_robot_driver",
        executable="ur_ros2_control_node",
        parameters=[
            update_rate_config_file,
            ParameterFile(initial_joint_controllers, allow_substs=True),
            robot_description,
        ],
        output="screen",
        condition=UnlessCondition(use_topic),
    )

    return LaunchDescription(
        [
            # Vars
            declare_use_topic,
            set_tf_prefix,
            # Nodes
            robot_state_publisher_node,
            ur_control_node_parameter,
            ur_control_node_topic,
        ]
    )

Expected Behavior

Not to crash/it should work the same with robot_description in a parameter or on a topic.

Actual Behavior

Segfault (see log output).

Workaround Suggestion

Don't use a topic.

Relevant log output

$ ros2 launch my_pkg test_launch.launch.py use_topic:=true
[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2024-02-14-15-43-40-255061-ros-desktop-108382
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [108384]
[INFO] [ur_ros2_control_node-2]: process started with pid [108386]
[robot_state_publisher-1] [INFO] [1707954220.613429558] [robot_state_publisher]: got segment base
[robot_state_publisher-1] [INFO] [1707954220.613659322] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1707954220.613679429] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-1] [INFO] [1707954220.613693354] [robot_state_publisher]: got segment flange
[robot_state_publisher-1] [INFO] [1707954220.613705664] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-1] [INFO] [1707954220.613716632] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-1] [INFO] [1707954220.613727331] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-1] [INFO] [1707954220.613738319] [robot_state_publisher]: got segment tool0
[robot_state_publisher-1] [INFO] [1707954220.613752175] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-1] [INFO] [1707954220.613763924] [robot_state_publisher]: got segment world
[robot_state_publisher-1] [INFO] [1707954220.613778452] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-1] [INFO] [1707954220.613791824] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-1] [INFO] [1707954220.613803639] [robot_state_publisher]: got segment wrist_3_link
[ur_ros2_control_node-2] [INFO] [1707954220.622437382] [controller_manager]: Subscribing to '~/robot_description' topic for robot description file.
[ur_ros2_control_node-2] [WARN] [1707954220.623552987] [controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-2] [INFO] [1707954220.634134441] [controller_manager]: Received robot description file.
[ur_ros2_control_node-2] text not specified in the tf_prefix tag
[ur_ros2_control_node-2] [INFO] [1707954220.634670371] [resource_manager]: Loading hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954220.636335707] [resource_manager]: Initialize hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954220.636385216] [resource_manager]: Successful initialization of hardware 'ur'
[ur_ros2_control_node-2] [WARN] [1707954220.636592047] [controller_manager]: [Deprecated]: Automatic activation of all hardware components will not be supported in the future anymore. Use hardware_spawner instead.
[ur_ros2_control_node-2] [INFO] [1707954220.636625767] [resource_manager]: 'configure' hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954220.636633329] [resource_manager]: Successful 'configure' of hardware 'ur'
[ur_ros2_control_node-2] [INFO] [1707954220.636657030] [resource_manager]: 'activate' hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954220.636668107] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-2] [INFO] [1707954220.636691537] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-2] [WARN] [1707954220.637410110] [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-2] Stack trace (most recent call last) in thread 108406:
[ur_ros2_control_node-2] #9    Object "", at 0xffffffffffffffff, in 
[ur_ros2_control_node-2] #8    Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in __clone3 [0x7f959372684f]
[ur_ros2_control_node-2] #7    Source "./nptl/pthread_create.c", line 442, in start_thread [0x7f9593694ac2]
[ur_ros2_control_node-2] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f9593adc252, in 
[ur_ros2_control_node-2] #5    Object "/opt/ros/humble/lib/ur_robot_driver/ur_ros2_control_node", at 0x5605dcb69648, in 
[ur_ros2_control_node-2] #4    Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7f9594063934, in controller_manager::ControllerManager::read(rclcpp::Time const&, rclcpp::Duration const&)
[ur_ros2_control_node-2] #3    Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f9593cbd69c, in hardware_interface::ResourceManager::read(rclcpp::Time const&, rclcpp::Duration const&)
[ur_ros2_control_node-2] #2    Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f9593cdae8f, in hardware_interface::System::read(rclcpp::Time const&, rclcpp::Duration const&)
[ur_ros2_control_node-2] #1    Object "/opt/ros/humble/lib/libur_robot_driver_plugin.so", at 0x7f958c102af3, in ur_robot_driver::URPositionHardwareInterface::read(rclcpp::Time const&, rclcpp::Duration const&)
[ur_ros2_control_node-2] #0    Object "/opt/ros/humble/lib/x86_64-linux-gnu/liburcl.so", at 0x7f9574fbb124, in urcl::UrDriver::startRTDECommunication()
[ur_ros2_control_node-2] Segmentation fault (Address not mapped to object [0x18])
[ERROR] [ur_ros2_control_node-2]: process has died [pid 108386, exit code -11, cmd '/opt/ros/humble/lib/ur_robot_driver/ur_ros2_control_node --ros-args --params-file /opt/ros/humble/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_zhwwng13 -r ~/robot_description:=robot_description'].

$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

$ ros2 launch my_pkg test_launch.launch.py use_topic:=false
[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2024-02-14-15-44-54-574273-ros-desktop-108423
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [108426]
[INFO] [ur_ros2_control_node-2]: process started with pid [108428]
[robot_state_publisher-1] [INFO] [1707954295.112506525] [robot_state_publisher]: got segment base
[robot_state_publisher-1] [INFO] [1707954295.112759828] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1707954295.112779339] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-1] [INFO] [1707954295.112790301] [robot_state_publisher]: got segment flange
[robot_state_publisher-1] [INFO] [1707954295.112801325] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-1] [INFO] [1707954295.112809562] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-1] [INFO] [1707954295.112820283] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-1] [INFO] [1707954295.112826550] [robot_state_publisher]: got segment tool0
[robot_state_publisher-1] [INFO] [1707954295.112834116] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-1] [INFO] [1707954295.112839823] [robot_state_publisher]: got segment world
[robot_state_publisher-1] [INFO] [1707954295.112845567] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-1] [INFO] [1707954295.112851259] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-1] [INFO] [1707954295.112857303] [robot_state_publisher]: got segment wrist_3_link
[ur_ros2_control_node-2] [WARN] [1707954295.125977396] [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-2] text not specified in the tf_prefix tag
[ur_ros2_control_node-2] [INFO] [1707954295.126391763] [resource_manager]: Loading hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954295.128072168] [resource_manager]: Initialize hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954295.128122733] [resource_manager]: Successful initialization of hardware 'ur'
[ur_ros2_control_node-2] [WARN] [1707954295.128277301] [controller_manager]: [Deprecated]: Automatic activation of all hardware components will not be supported in the future anymore. Use hardware_spawner instead.
[ur_ros2_control_node-2] [INFO] [1707954295.128300251] [resource_manager]: 'configure' hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954295.128307874] [resource_manager]: Successful 'configure' of hardware 'ur'
[ur_ros2_control_node-2] [INFO] [1707954295.128331640] [resource_manager]: 'activate' hardware 'ur' 
[ur_ros2_control_node-2] [INFO] [1707954295.128343106] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-2] [INFO] [1707954295.128354026] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-2] [WARN] [1707954295.128956698] [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-2] [INFO] [1707954295.575642183] [UR_Client_Library:]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-2] [INFO] [1707954295.576014970] [UR_Client_Library:]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-2] [WARN] [1707954296.600732736] [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-2] [WARN] [1707954296.600770473] [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-2] [INFO] [1707954296.600791785] [URPositionHardwareInterface]: Calibration checksum: 'calib_12788084448423163542'.
[ur_ros2_control_node-2] [ERROR] [1707954297.694331192] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_calibration/README.md] for details.
[ur_ros2_control_node-2] [INFO] [1707954297.694428555] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-2] [INFO] [1707954297.694459343] [resource_manager]: Successful 'activate' of hardware 'ur'
[ur_ros2_control_node-2] [WARN] [1707954297.699331089] [controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-2] [WARN] [1707954297.699511046] [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.

Accept Public visibility

fmauch commented 8 months ago

That should hopefully be fixed by https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/pull/936. @mrjogo could you please check that?

mrjogo commented 8 months ago

That seems to fix it! Thanks.