search

ros-controls / ros2_control

Generic and simple controls framework for ROS 2
https://control.ros.org
Apache License 2.0
433 stars 272 forks source link

[Error] ros2_control tag not recognized in URDF #1602

Closed kbs907 closed 4 days ago

kbs907 commented 4 days ago

Describe the bug I am currently trying to spawn a robot in Gazebo. Although I am using the ros2_control tag in the URDF, "no ros2_control tag" error occured.

Environment (please complete the following information):

Terminal output content The following content is the output displayed in the terminal when I run the launch file.

[INFO] [launch]: All log files can be found below /home/kbs-2204/.ros/log/2024-07-04-13-15-01-230958-kbs2204-Dell-G15-5520-6216
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [6218]
[INFO] [gzserver-2]: process started with pid [6220]
[INFO] [gzclient-3]: process started with pid [6222]
[INFO] [bash-4]: process started with pid [6224]
[INFO] [robot_control.py-5]: process started with pid [6226]
[INFO] [ros2-6]: process started with pid [6229]
[INFO] [ros2-7]: process started with pid [6231]
[INFO] [ros2-8]: process started with pid [6233]
[robot_state_publisher-1] [INFO] [1720066502.439677906] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-1] [INFO] [1720066502.439795991] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1720066502.439805131] [robot_state_publisher]: got segment body_link
[robot_state_publisher-1] [INFO] [1720066502.439810789] [robot_state_publisher]: got segment body_top
[robot_state_publisher-1] [INFO] [1720066502.439815853] [robot_state_publisher]: got segment front_lidar
[robot_state_publisher-1] [INFO] [1720066502.439820733] [robot_state_publisher]: got segment front_lidar_assy
[robot_state_publisher-1] [INFO] [1720066502.439826064] [robot_state_publisher]: got segment front_rs_camera_bottom_screw_frame
[robot_state_publisher-1] [INFO] [1720066502.439831683] [robot_state_publisher]: got segment front_rs_camera_color_frame
[robot_state_publisher-1] [INFO] [1720066502.439837130] [robot_state_publisher]: got segment front_rs_camera_color_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439842360] [robot_state_publisher]: got segment front_rs_camera_depth_frame
[robot_state_publisher-1] [INFO] [1720066502.439847617] [robot_state_publisher]: got segment front_rs_camera_depth_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439852632] [robot_state_publisher]: got segment front_rs_camera_left_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439857692] [robot_state_publisher]: got segment front_rs_camera_left_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439862688] [robot_state_publisher]: got segment front_rs_camera_link
[robot_state_publisher-1] [INFO] [1720066502.439867890] [robot_state_publisher]: got segment front_rs_camera_right_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439872977] [robot_state_publisher]: got segment front_rs_camera_right_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439877969] [robot_state_publisher]: got segment holder1
[robot_state_publisher-1] [INFO] [1720066502.439883209] [robot_state_publisher]: got segment holder2
[robot_state_publisher-1] [INFO] [1720066502.439888139] [robot_state_publisher]: got segment holder3
[robot_state_publisher-1] [INFO] [1720066502.439892924] [robot_state_publisher]: got segment holder4
[robot_state_publisher-1] [INFO] [1720066502.439897959] [robot_state_publisher]: got segment left_rs_camera_bottom_screw_frame
[robot_state_publisher-1] [INFO] [1720066502.439902987] [robot_state_publisher]: got segment left_rs_camera_color_frame
[robot_state_publisher-1] [INFO] [1720066502.439907856] [robot_state_publisher]: got segment left_rs_camera_color_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439912841] [robot_state_publisher]: got segment left_rs_camera_depth_frame
[robot_state_publisher-1] [INFO] [1720066502.439917901] [robot_state_publisher]: got segment left_rs_camera_depth_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439923078] [robot_state_publisher]: got segment left_rs_camera_left_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439927933] [robot_state_publisher]: got segment left_rs_camera_left_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439932883] [robot_state_publisher]: got segment left_rs_camera_link
[robot_state_publisher-1] [INFO] [1720066502.439938067] [robot_state_publisher]: got segment left_rs_camera_right_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439942986] [robot_state_publisher]: got segment left_rs_camera_right_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439947962] [robot_state_publisher]: got segment rear_lidar
[robot_state_publisher-1] [INFO] [1720066502.439952665] [robot_state_publisher]: got segment rear_lidar_assy
[robot_state_publisher-1] [INFO] [1720066502.439957281] [robot_state_publisher]: got segment rear_rs_camera_bottom_screw_frame
[robot_state_publisher-1] [INFO] [1720066502.439961819] [robot_state_publisher]: got segment rear_rs_camera_color_frame
[robot_state_publisher-1] [INFO] [1720066502.439966222] [robot_state_publisher]: got segment rear_rs_camera_color_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439970853] [robot_state_publisher]: got segment rear_rs_camera_depth_frame
[robot_state_publisher-1] [INFO] [1720066502.439975470] [robot_state_publisher]: got segment rear_rs_camera_depth_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439980252] [robot_state_publisher]: got segment rear_rs_camera_left_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439985018] [robot_state_publisher]: got segment rear_rs_camera_left_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.439989880] [robot_state_publisher]: got segment rear_rs_camera_link
[robot_state_publisher-1] [INFO] [1720066502.439994914] [robot_state_publisher]: got segment rear_rs_camera_right_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.439999676] [robot_state_publisher]: got segment rear_rs_camera_right_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.440004622] [robot_state_publisher]: got segment right_rs_camera_bottom_screw_frame
[robot_state_publisher-1] [INFO] [1720066502.440009325] [robot_state_publisher]: got segment right_rs_camera_color_frame
[robot_state_publisher-1] [INFO] [1720066502.440014319] [robot_state_publisher]: got segment right_rs_camera_color_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.440722785] [robot_state_publisher]: got segment right_rs_camera_depth_frame
[robot_state_publisher-1] [INFO] [1720066502.440754506] [robot_state_publisher]: got segment right_rs_camera_depth_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.440794426] [robot_state_publisher]: got segment right_rs_camera_left_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.440800132] [robot_state_publisher]: got segment right_rs_camera_left_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.440839527] [robot_state_publisher]: got segment right_rs_camera_link
[robot_state_publisher-1] [INFO] [1720066502.440844609] [robot_state_publisher]: got segment right_rs_camera_right_ir_frame
[robot_state_publisher-1] [INFO] [1720066502.440884774] [robot_state_publisher]: got segment right_rs_camera_right_ir_optical_frame
[robot_state_publisher-1] [INFO] [1720066502.440909521] [robot_state_publisher]: got segment wheel1
[robot_state_publisher-1] [INFO] [1720066502.440929608] [robot_state_publisher]: got segment wheel2
[robot_state_publisher-1] [INFO] [1720066502.440955149] [robot_state_publisher]: got segment wheel3
[robot_state_publisher-1] [INFO] [1720066502.440977481] [robot_state_publisher]: got segment wheel4
[bash-4] [INFO] [1720066505.302657953] [spawn_entity]: Spawn Entity started
[bash-4] [INFO] [1720066505.302948914] [spawn_entity]: Loading entity published on topic /robot_description
[bash-4] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[bash-4]   warnings.warn(
[bash-4] [INFO] [1720066505.304204807] [spawn_entity]: Waiting for entity xml on /robot_description
[bash-4] [INFO] [1720066505.327698220] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[bash-4] [INFO] [1720066505.328106778] [spawn_entity]: Waiting for service /spawn_entity
[bash-4] [INFO] [1720066505.337544661] [spawn_entity]: Calling service /spawn_entity
[bash-4] [INFO] [1720066505.790071992] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [thirabot]
[INFO] [bash-4]: process has finished cleanly [pid 6224]
[gzserver-2] [INFO] [1720066506.267105366] [four_ws_robot_joint_state]: Going to publish joint [wheel1_joint]
[gzserver-2] [INFO] [1720066506.267311900] [four_ws_robot_joint_state]: Going to publish joint [wheel2_joint]
[gzserver-2] [INFO] [1720066506.267318790] [four_ws_robot_joint_state]: Going to publish joint [wheel3_joint]
[gzserver-2] [INFO] [1720066506.267322840] [four_ws_robot_joint_state]: Going to publish joint [wheel4_joint]
[gzserver-2] [INFO] [1720066506.267325826] [four_ws_robot_joint_state]: Going to publish joint [holder1_joint]
[gzserver-2] [INFO] [1720066506.267328673] [four_ws_robot_joint_state]: Going to publish joint [holder2_joint]
[gzserver-2] [INFO] [1720066506.267331384] [four_ws_robot_joint_state]: Going to publish joint [holder3_joint]
[gzserver-2] [INFO] [1720066506.267334222] [four_ws_robot_joint_state]: Going to publish joint [holder4_joint]
[gzserver-2] [INFO] [1720066506.280522002] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-2] [INFO] [1720066506.282925590] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-2] [INFO] [1720066506.282956883] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-2] [INFO] [1720066506.285185557] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-2] [INFO] [1720066506.285593645] [gazebo_ros2_control]: Received urdf from param server, parsing...
[gzserver-2] [INFO] [1720066506.285632065] [gazebo_ros2_control]: Loading parameter files /home/kbs-2204/simulation_ws/install/gazebo_description/share/gazebo_description/config/swerve_controllers.yaml
[gzserver-2] [ERROR] [1720066506.286847432] [gazebo_ros2_control]: Error parsing URDF in gazebo_ros2_control plugin, plugin not active : no ros2_control tag
[ros2-6] Could not contact service /controller_manager/load_controller
[ERROR] [ros2-6]: process has died [pid 6229, exit code 1, cmd 'ros2 control load_controller --set-state active forward_position_controller'].
[ros2-8] Could not contact service /controller_manager/load_controller
[ros2-7] Could not contact service /controller_manager/load_controller
[ERROR] [ros2-7]: process has died [pid 6231, exit code 1, cmd 'ros2 control load_controller --set-state active forward_velocity_controller'].
[ERROR] [ros2-8]: process has died [pid 6233, exit code 1, cmd 'ros2 control load_controller --set-state active joint_state_broadcaster'].

swerve.urdf.xacro I am running the swerve.urdf.xacro file, which includes the swerve.gazebo file using the include command.

The following content is only the relevant part of the URDF file related to the issue.

  <xacro:macro name="ros2_control_steering_joint" params="joint_name">
    <joint name="${joint_name}">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </xacro:macro>

  <xacro:macro name="ros2_control_wheel_joint" params="joint_name">
    <joint name="${joint_name}">
      <command_interface name="velocity">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </xacro:macro>

  <gazebo>
    <ros2_control name="GazeboSystem" type="system">
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>

      <xacro:ros2_control_steering_joint joint_name="holder1_joint"/>
      <xacro:ros2_control_wheel_joint joint_name="wheel1_joint"/>
      <xacro:ros2_control_steering_joint joint_name="holder2_joint"/>
      <xacro:ros2_control_wheel_joint joint_name="wheel2_joint"/>
      <xacro:ros2_control_steering_joint joint_name="holder3_joint"/>
      <xacro:ros2_control_wheel_joint joint_name="wheel3_joint"/> 
      <xacro:ros2_control_steering_joint joint_name="holder4_joint"/>
      <xacro:ros2_control_wheel_joint joint_name="wheel4_joint"/>

    </ros2_control>
  </gazebo>

swerve.gazebo

  <gazebo>
    <plugin name="four_ws_robot_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
      <ros>
          <remapping>~/out:=joint_states</remapping>
      </ros>
      <update_rate>30</update_rate>
      <joint_name>wheel1_joint</joint_name>
      <joint_name>wheel2_joint</joint_name>
      <joint_name>wheel3_joint</joint_name>
      <joint_name>wheel4_joint</joint_name>
      <joint_name>holder1_joint</joint_name>
      <joint_name>holder2_joint</joint_name>
      <joint_name>holder3_joint</joint_name>
      <joint_name>holder4_joint</joint_name>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
      <parameters>$(find gazebo_description)/config/swerve_controllers.yaml</parameters>
    </plugin>
  </gazebo>

launch file

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

WORLD_MODEL = 'no_roof_small_warehouse.world'
URDF_FILE_NAME = 'swerve.urdf.xacro' #'rs_robot.urdf'

def generate_launch_description():
    # Get the directory of the gazebo_world package
    gazebo_world_share_dir = get_package_share_directory('gazebo_world')
    gazebo_bringup_share_dir = get_package_share_directory('gazebo_bringup')
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    swerve_steering_controller_dir = get_package_share_directory('swerve_steering_control')

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

    x_pose = LaunchConfiguration('x_pose', default='-2.0')
    y_pose = LaunchConfiguration('y_pose', default='-0.5')

    gazebo_model_path = SetEnvironmentVariable(
        name='GAZEBO_MODEL_PATH',
        value=f"{get_package_share_directory('gazebo_description')}/models:{get_package_share_directory('common_urdf')}/models"
    )

    world = os.path.join(
        gazebo_world_share_dir,
        'worlds',
        WORLD_MODEL
    )

    urdf_path = os.path.join(
        get_package_share_directory('gazebo_description'),
        'urdf',
        URDF_FILE_NAME)
    robot_description_config = xacro.process_file(urdf_path)
    robot_desc = robot_description_config.toxml()

    gzserver_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
        ),
        launch_arguments={'world': world}.items()
    )

    gzclient_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
        )
    )

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'use_sim_time': use_sim_time,
            'robot_description': robot_desc
        }],
        arguments=[urdf_path],
    )

    spawn_robot_node = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        output='screen',
        arguments=['-entity', 'thirabot',
                   '-x', '0',
                   '-y', '0',
                   '-z', '0',
                   '-R', '0',
                   '-P', '0',
                   '-Y', '0',
                   '-topic', '/robot_description'],
        prefix = ["bash -c 'sleep 2.0; $0 $@' "]
    )

    controller = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(swerve_steering_controller_dir, 'launch', 'swerve_steering_control.launch.py')
            )
        )

    forward_position_controller = ExecuteProcess( 
            cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'forward_position_controller'], output='screen'
        )

    forward_velocity_controller = ExecuteProcess(
            cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'forward_velocity_controller'], output='screen'
        )

    joint_state_broadcaster = ExecuteProcess(
            cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen'
        )

    nodes = [
        gazebo_model_path,
        robot_state_publisher_node,
        gzserver_cmd,
        gzclient_cmd,
        spawn_robot_node,
        controller,
        forward_position_controller,
        forward_velocity_controller,
        joint_state_broadcaster
    ]

    return LaunchDescription(nodes)

Even though I am using the ros2_control tag, I am experiencing this error, which is causing me a lot of distress. I have tried checking and attempting various things, but I am still unable to figure out the root cause. If anyone could help me, I would be extremely grateful.

kbs907 commented 4 days ago

Oh my god.. I removed the 'gazebo' tag that was wrapping it, and now it started recognizing the 'ros2_control' tag.

bmagyar commented 3 days ago

Sometimes you only need a rubber duck to explain a problem to ;)