Robotics StackExchange | Archived questions

How can I identify the node/plugin that is broadcasting a specific TF transform in ROS2?

I am currently configuring a mobile base robot model in Xacro and I have created a launch file to spawn two robots into Gazebo. However, my TF tree does not look correct, and I want to clean it up by deactivating the broadcaster that is affecting the "left_wheel" and "right_wheel" frames.

When I run ros2 run rqt_tf_tree rqt_tf_tree, I can see the entire TF tree, but I am unsure how to find the specific broadcaster that is affecting these frames.

image description

Here is a closeup image:

image description

I have tried using the "tf2monitor" command in ROS2 (`ros2 run tf2ros tf2monitor), but it only shows the list of all frames and their publishers. Runningros2 run tf2ros tf2echo baselink rightwheelonly shows the translation and rotation data, but not the broadcasters. When I runros2 topic info /tf -v`, it only provides general information about the broadcasters and listeners but does not give me the specific information I need. I would appreciate any guidance on how to identify the broadcaster that is affecting the "**leftwheel" and "right_wheel**" frames.

Thank you,

Roberto


The details, if needed:

ros2 run tf2_ros tf2_monitor

Only shows:

RESULTS: for all Frames

Frames:
Frame: left_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: right_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/back_pitch_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/back_roll_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/back_yaw_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/chassis, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/front_pitch_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/front_roll_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/front_yaw_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/left_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot1/right_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/back_pitch_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/back_roll_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/back_yaw_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/chassis, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/front_pitch_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/front_roll_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/front_yaw_link, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/left_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09
Frame: robot2/right_wheel, published by <no authority available>, Average Delay: 1.67957e+09, Max Delay: 1.67957e+09

All Broadcasters:
Node: <no authority available> 229.529 Hz, Average Delay: 1.67957e+09 Max Delay: 1.67957e+09

For completeness this is the plugin part in my XACRO file:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

<xacro:property name="robot_name" value="$(arg robot_name)"/>

....
.....
.....

  <gazebo>
    <plugin name="box_bot_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
      <ros>
          <namespace>/${robot_name}</namespace>
          <remapping>~/out:=joint_states</remapping>
      </ros>
      <update_rate>30</update_rate>

      <joint_name>joint_left_wheel</joint_name>
      <joint_name>joint_right_wheel</joint_name>
      <joint_name>front_yaw_joint</joint_name>
      <joint_name>back_yaw_joint</joint_name>
      <joint_name>front_roll_joint</joint_name>
      <joint_name>back_roll_joint</joint_name>
      <joint_name>front_pitch_joint</joint_name>
      <joint_name>back_pitch_joint</joint_name>

    </plugin>
  </gazebo>

  <gazebo>
<plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
  <ros>
    <namespace>/${robot_name}</namespace>
    <remapping>/cmd_vel:=cmd_vel</remapping>
  </ros>

  <update_rate>100</update_rate>
  <left_joint>joint_left_wheel</left_joint>
  <right_joint>joint_right_wheel</right_joint>
  <wheel_separation>0.1</wheel_separation>
  <wheel_diameter>0.07</wheel_diameter>
  <publish_odom>true</publish_odom>
  <odometry_frame>odom</odometry_frame>>
  <robot_base_frame>base_link</robot_base_frame>
  <publish_odom_tf>false</publish_odom_tf>
  <publish_wheel_tf>true</publish_wheel_tf>
  <max_wheel_torque>1.0</max_wheel_torque>
  <max_wheel_acceleration>2.0</max_wheel_acceleration>

  </plugin>
  </gazebo>

</robot>

And this is the launch file:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.substitutions import Command, LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    world_file_arg = DeclareLaunchArgument(
        'world',
        default_value=[get_package_share_directory(
            'my_robot'), '/worlds/empty.world'],
    )

    gazebo_launch_args = {
        'verbose': 'false',
        'pause': 'false',
        'world': LaunchConfiguration('world')
    }

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
        launch_arguments=gazebo_launch_args.items(),
    )

    robot_desc_path = os.path.join(get_package_share_directory(
        "my_robot_description"), "urdf", "my_robot.xacro"

    robot_name_1 = "robot1"
    robot_name_2 = "robot2"

    rsp_robot1 = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        namespace=robot_name_1,
        parameters=[{'frame_prefix': robot_name_1+'/', 'use_sim_time': use_sim_time,
                     'robot_description': Command(['xacro ', robot_desc_path, ' robot_name:=', robot_name_1])}],
        output="screen"
    )

    rsp_robot2 = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        namespace=robot_name_2,
        parameters=[{'frame_prefix': robot_name_2+'/', 'use_sim_time': use_sim_time,
                     'robot_description': Command(['xacro ', robot_desc_path, ' robot_name:=', robot_name_2])}],
        output="screen"
    )

    spawn_robot1 = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'robot1', '-x', '0.0', '-y', '0.0', '-z', '0.0',
                   '-topic', robot_name_1+'/robot_description']
    )

    spawn_robot2 = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'robot2', '-x', '1.0', '-y', '1.0', '-z', '0.0',
                   '-topic', robot_name_2+'/robot_description']
    )

    return LaunchDescription([
        world_file_arg,
        gazebo,
        rsp_robot1,
        rsp_robot2,
        spawn_robot1,
        spawn_robot2
    ])

Asked by Roberto Z. on 2023-03-23 05:45:38 UTC

Comments

Answers