UR driver and Clearpath Husky ros2 driver doesn't work simultaneously

Affected ROS2 Driver version(s)

Humble

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR CB3 robot, Real robot

Robot SW / URSim version(s)

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

I have a mobile robotic arm composed of a ur3 robotic arm and a husky mobile robot. When I use Universal_Robots_ROS2_Driver or Clearpath Husky ROS2 driver alone, they both work fine.I can use moveit2 to control ur arm and navigation2 to control husky mobile robot.

Then, I created a new mobile manipulator description file urdf based on the UR description package and Husky description package, and used the ur_robot_driver package in the launch file to load the robot model and ros2 control manager spawn ur controllers(I didn't spawn husky controller ). I used the ros2 control list_contollers command and the ros2 control list_hardware_components command to show the hardware and controller status, they both work fine.

Howerver, when I press the play button on the teach pendant, the linux terminal keeps displaying the following information:

Connection to reverse interface dropped.
Robot connected to reverse interface. Ready to receive control commands.

After a while, the terminal may also display error:

[UR_Client_Library]: Pipeline producer overflowed!

And what's worse is the uncontrolled movement of the ur3 arm. Horrible!

When I delete the ros2 control tag about Huksy in the urdf description file, the ur3 robot arm can work normally again.

Husky ros2 control tag:

    <ros2_control name="${prefix}husky_hardware" type="system">
      <hardware>
        <xacro:if value="$(arg is_sim)">
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </xacro:if>
        <xacro:unless value="$(arg is_sim)">
          <plugin>husky_base/HuskyHardware</plugin>
          <plugin>fake_components/GenericSystem</plugin>
          <param name="hw_start_duration_sec">2.0</param>
          <param name="hw_stop_duration_sec">3.0</param>
          <param name="wheel_diameter">0.3302</param>
          <param name="max_accel">5.0</param>
          <param name="max_speed">1.0</param>
          <param name="polling_timeout">0.1</param>
          <param name="serial_port">$(arg serial_port)</param>
        </xacro:unless>
      </hardware>
      <joint name="${prefix}front_left_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}rear_left_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}front_right_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}rear_right_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
    </ros2_control>

UR3 is connected by ethernet and Husky is connected by serial port. I didn't install RT kernel.

How can I resolve this?

Relevant log output

No response

Accept Public visibility

[X] I agree to make this context public

Since we'll be migrating our Husky with a UR5 to Humble soon, I will be tackling this myself in the near future. For now I cannot say very much without seeing your complete URDF model / controller manager setup. At which frequency does your controller manager run?

Since we'll be migrating our Husky with a UR5 to Humble soon, I will be tackling this myself in the near future. For now I cannot say very much without seeing your complete URDF model / controller manager setup. At which frequency does your controller manager run?

The controller manager run at 125Hz. my robot description file:

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="huksy_ur3">
   <!-- robot name parameter -->
   <xacro:arg name="name" default="ur"/>
   <!-- import main macro -->
   <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
   <xacro:include filename="$(find husky_description)/urdf/husky_macro.urdf.xacro"/>

   <!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
   <!-- the default value should raise an error in case this was called without defining the type -->
   <xacro:arg name="ur_type" default="ur5x"/>

   <!-- parameters -->
   <xacro:arg name="tf_prefix" default="" />
   <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
   <xacro:arg name="kinematics_params" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
   <xacro:arg name="physical_params" default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
   <xacro:arg name="visual_params" default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
   <xacro:arg name="transmission_hw_interface" default=""/>
   <xacro:arg name="safety_limits" default="false"/>
   <xacro:arg name="safety_pos_margin" default="0.15"/>
   <xacro:arg name="safety_k_position" default="20"/>
   <!-- ros2_control related parameters -->
   <xacro:arg name="headless_mode" default="false" />
   <xacro:arg name="robot_ip" default="0.0.0.0" />
   <xacro:arg name="script_filename" default=""/>
   <xacro:arg name="output_recipe_filename" default=""/>
   <xacro:arg name="input_recipe_filename" default=""/>
   <xacro:arg name="reverse_ip" default="0.0.0.0"/>
   <xacro:arg name="script_command_port" default="50004"/>
   <!--   tool communication related parameters-->
   <xacro:arg name="use_tool_communication" default="false" />
   <xacro:arg name="tool_voltage" default="0" />
   <xacro:arg name="tool_parity" default="0" />
   <xacro:arg name="tool_baud_rate" default="115200" />
   <xacro:arg name="tool_stop_bits" default="1" />
   <xacro:arg name="tool_rx_idle_chars" default="1.5" />
   <xacro:arg name="tool_tx_idle_chars" default="3.5" />
   <xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
   <xacro:arg name="tool_tcp_port" default="54321" />

     <!-- Simulation parameters -->
   <xacro:arg name="use_fake_hardware" default="false" />
   <xacro:arg name="fake_sensor_commands" default="false" />
   <xacro:arg name="sim_gazebo" default="false" />
   <xacro:arg name="sim_ignition" default="false" />
   <xacro:arg name="simulation_controllers" default="" />

   <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
   <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>

   <!-- convert to property to use substitution in function -->
   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>

    <!-- create link fixed to the "husky_ur3_virtual_link" -->
    <link name="husky_ur3_virtual_link" />

    <joint name="husky_ur3_joint" type="fixed">
    <parent link="top_plate_link" />
    <child link = "husky_ur3_virtual_link" />
        <origin xyz="0.25 0 0.0065" rpy="0.0 0.0 -1.57" />
    </joint>

   <!-- arm -->
   <xacro:ur_robot
     name="$(arg name)"
     tf_prefix="$(arg tf_prefix)"
     parent="husky_ur3_virtual_link"
     joint_limits_parameters_file="$(arg joint_limit_params)"
     kinematics_parameters_file="$(arg kinematics_params)"
     physical_parameters_file="$(arg physical_params)"
     visual_parameters_file="$(arg visual_params)"
     transmission_hw_interface="$(arg transmission_hw_interface)"
     safety_limits="$(arg safety_limits)"
     safety_pos_margin="$(arg safety_pos_margin)"
     safety_k_position="$(arg safety_k_position)"
     use_fake_hardware="$(arg use_fake_hardware)"
     fake_sensor_commands="$(arg fake_sensor_commands)"
     sim_gazebo="$(arg sim_gazebo)"
     sim_ignition="$(arg sim_ignition)"
     headless_mode="$(arg headless_mode)"
     initial_positions="${xacro.load_yaml(initial_positions_file)}"
     use_tool_communication="$(arg use_tool_communication)"
     tool_voltage="$(arg tool_voltage)"
     tool_parity="$(arg tool_parity)"
     tool_baud_rate="$(arg tool_baud_rate)"
     tool_stop_bits="$(arg tool_stop_bits)"
     tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
     tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
     tool_device_name="$(arg tool_device_name)"
     tool_tcp_port="$(arg tool_tcp_port)"
     robot_ip="$(arg robot_ip)"
     script_filename="$(arg script_filename)"
     output_recipe_filename="$(arg output_recipe_filename)"
     input_recipe_filename="$(arg input_recipe_filename)"
     reverse_ip="$(arg reverse_ip)"
     script_command_port="$(arg script_command_port)"
     >
     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
   </xacro:ur_robot>

</robot>

I set tf_prefix:=ur_ . TF tree base_link->top_plate_link->husky_ur3_virtual_link->ur_base_link.

controller manager parameter:

controller_manager:
  ros__parameters:
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    husky_velocity_controller:
      type: diff_drive_controller/DiffDriveController

    io_and_status_controller:
      type: ur_controllers/GPIOController

    speed_scaling_state_broadcaster:
      type: ur_controllers/SpeedScalingStateBroadcaster

    force_torque_sensor_broadcaster:
      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    scaled_joint_trajectory_controller:
      type: ur_controllers/ScaledJointTrajectoryController

    forward_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController

    forward_position_controller:
      type: position_controllers/JointGroupPositionController

husky_velocity_controller:
  ros__parameters:
    left_wheel_names: ["front_left_wheel_joint", "rear_left_wheel_joint"]
    right_wheel_names: ["front_right_wheel_joint", "rear_right_wheel_joint"]

    wheel_separation: 0.512 #0.1  # 0.256  # 0.512
    wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
    wheel_radius: 0.1651 # 0.015

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

    open_loop: false
    enable_odom_tf: false

    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false
    #velocity_rolling_window_size: 10

    # Preserve turning radius when limiting speed/acceleration/jerk
    preserve_turning_radius: true

    # Publish limited velocity
    publish_cmd: true

    # Publish wheel data
    publish_wheel_data: true

    # Velocity and acceleration limits
    # Whenever a min_* is unspecified, default to -max_*
    linear.x.has_velocity_limits: true
    linear.x.has_acceleration_limits: true
    linear.x.has_jerk_limits: false
    linear.x.max_velocity: 1.0
    linear.x.min_velocity: -1.0
    linear.x.max_acceleration: 1.0
    linear.x.max_jerk: 0.0
    linear.x.min_jerk: 0.0

    angular.z.has_velocity_limits: true
    angular.z.has_acceleration_limits: true
    angular.z.has_jerk_limits: false
    angular.z.max_velocity: 1.0
    angular.z.min_velocity: -1.0
    angular.z.max_acceleration: 1.0
    angular.z.min_acceleration: -1.0
    angular.z.max_jerk: 0.0
    angular.z.min_jerk: 0.0

speed_scaling_state_broadcaster:
  ros__parameters:
    state_publish_rate: 100.0
    tf_prefix: "ur_"

io_and_status_controller:
  ros__parameters:
    tf_prefix: "ur_"

force_torque_sensor_broadcaster:
  ros__parameters:
    sensor_name: ur_tcp_fts_sensor
    state_interface_names:
      - force.x
      - force.y
      - force.z
      - torque.x
      - torque.y
      - torque.z
    frame_id: ur_tool0
    topic_name: ft_data

joint_trajectory_controller:
  ros__parameters:
    joints:
      - ur_shoulder_pan_joint
      - ur_shoulder_lift_joint
      - ur_elbow_joint
      - ur_wrist_1_joint
      - ur_wrist_2_joint
      - ur_wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      ur_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      ur_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      ur_elbow_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }

scaled_joint_trajectory_controller:
  ros__parameters:
    joints:
      - ur_shoulder_pan_joint
      - ur_shoulder_lift_joint
      - ur_elbow_joint
      - ur_wrist_1_joint
      - ur_wrist_2_joint
      - ur_wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      ur_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      ur_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      ur_elbow_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      ur_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
    speed_scaling_interface_name: ur_speed_scaling/speed_scaling_factor

forward_velocity_controller:
  ros__parameters:
    joints:
      - ur_shoulder_pan_joint
      - ur_shoulder_lift_joint
      - ur_elbow_joint
      - ur_wrist_1_joint
      - ur_wrist_2_joint
      - ur_wrist_3_joint
    interface_name: velocity

forward_position_controller:
  ros__parameters:
    joints:
      - ur_shoulder_pan_joint
      - ur_shoulder_lift_joint
      - ur_elbow_joint
      - ur_wrist_1_joint
      - ur_wrist_2_joint
      - ur_wrist_3_joint

Control manager node:

    ur_control_node = 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),
    )

I only changed robot_description.

Hi,

I had something similar working with an UR10e and a RobotiQ Hand-E gripper with ROS2 Humble. I was just wondering if I should start an issue of my own or not.

Both drivers worked fine on their own ; but when starting both with the same controller_manager, the UR started piling up some data in its queue, resulting in a Producer pipeline overflowed issue (as stated here ) and then showed instabilities and chaotic behavior. At the time I first tried using 2 different controller_managers and opened an issue on the ros2_control repo.

I tried quite a lot of things with is_async parameters (both on the controller and the hardware sides), control loop rates. In the end, I noticed I had a blocking read function in my RobotiQ hardware_interface. Once I changed it, I didn't have the Producer pipeline overflowed issue anymore. I then noticed it worked well with low speed_scaling_factors but not so much after that (50% +). Though I didn't have the Producer pipeline overflowed error, I had some logs on my TeachPendant stating RT Machine: socket read binary integer timeout which comes from the urscript line:

params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout)

It once again resulted in unstable and jerky motions that sometimes ended up with a protective stop. I ended up following ros2_control advice to have different update rates on my gripper and UR10e, so that I could lower the update rate of the gripper.

I now can go up to 100% speed_scaling_factor, though I still see some socket read binary integer timeouts on the TeachPendant.

I hope this can help, though I was wondering if this was a good solution. As I was having a more in-depth look into this package hardware_interface.cpp, I realized there was a non_blocking_read attribute. Should it be used ? It feels like a ROS1 heritage, as a comment before refers to combined_robot_hw. Otherwise would a RT Kernel solve these timeouts ?

Thanks, Lucas

Hello @LucasLabarussiat,
Thanks for your answer. I have installed PREEMPT RT on my system, but it cannot solve this issue. For now, I write a ros2 action server which can receive moveit2 command and send urscript to control ur3 by urx library. But over the next few weeks I'll try to lower the update rate of husky. Hope it helps.

Hi,

Would you mind sharing the code of the husky hardware_interface.cpp so that we can check whether the problem comes from there ?

@LucasLabarussiat You can download husky source code from https://github.com/husky/husky/tree/humble-devel husky hardware_interface.cpp is located in husky_base package.

@LucasLabarussiat In this case the non_blocking_read has to be used (same goes for multiarm setups). Otherwise the hw_interface waits for the next UR RTDE package.

Hi,

I had something similar working with an UR10e and a RobotiQ Hand-E gripper with ROS2 Humble. I was just wondering if I should start an issue of my own or not.

Both drivers worked fine on their own ; but when starting both with the same controller_manager, the UR started piling up some data in its queue, resulting in a Producer pipeline overflowed issue (as stated here ) and then showed instabilities and chaotic behavior. At the time I first tried using 2 different controller_managers and opened an issue on the ros2_control repo.

I tried quite a lot of things with is_async parameters (both on the controller and the hardware sides), control loop rates. In the end, I noticed I had a blocking read function in my RobotiQ hardware_interface. Once I changed it, I didn't have the Producer pipeline overflowed issue anymore. I then noticed it worked well with low speed_scaling_factors but not so much after that (50% +). Though I didn't have the Producer pipeline overflowed error, I had some logs on my TeachPendant stating RT Machine: socket read binary integer timeout which comes from the urscript line:
params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout)
It once again resulted in unstable and jerky motions that sometimes ended up with a protective stop. I ended up following ros2_control advice to have different update rates on my gripper and UR10e, so that I could lower the update rate of the gripper.

I now can go up to 100% speed_scaling_factor, though I still see some socket read binary integer timeouts on the TeachPendant.

I hope this can help, though I was wondering if this was a good solution. As I was having a more in-depth look into this package hardware_interface.cpp, I realized there was a non_blocking_read attribute. Should it be used ? It feels like a ROS1 heritage, as a comment before refers to combined_robot_hw. Otherwise would a RT Kernel solve these timeouts ?

Thanks, Lucas

Hi Lucas, May I ask if you were running the robot and the gripper with the same IP? I am using a UR5 with a robotiq gripper, both with RTDE and the same IP. I am facing the same problem once you had. But I am struggling configuring the robotiq gripper driver to non-blocking mode.

UniversalRobots / Universal_Robots_ROS2_Driver