search

ros-controls / gz_ros2_control

Connect the latest version of Gazebo with ros2_control.
https://gazebosim.org
Apache License 2.0
112 stars 85 forks source link

Using ros2_control in SDF Causes Errors #419

Closed Amronos closed 2 weeks ago

Amronos commented 2 weeks ago

The following errors happen upon using the <ros2_control> tag in an SDF file and passing the SDF file to robot_state_publisher, alongside using that same SDF file to include a model into the Gazebo world using the <include> tag.

[INFO] [ruby $(which gz) sim-1]: process started with pid [36843]
[INFO] [parameter_bridge-2]: process started with pid [36844]
[INFO] [robot_state_publisher-3]: process started with pid [36846]
[INFO] [rviz2-4]: process started with pid [36847]
[robot_state_publisher-3] Warning [Utils.cc:132] [/sdf/model[@name="my_robot"]/ros2_control[@name="GazeboSimSystem"]:<data-string>:L201]: XML Element[ros2_control], child of element[model], not defined in SDF. Copying[ros2_control] as children of [model].
[robot_state_publisher-3] [WARN] [1727185581.609845252] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-3] [INFO] [1727185581.626799775] [robot_state_publisher]: Robot initialized
[ruby $(which gz) sim-1] Warning [Utils.cc:132] [/sdf/model[@name="my_robot"]/ros2_control[@name="GazeboSimSystem"]:/home/amronos/ros2_ws/install/my_robot/share/my_robot/models/my_robot/model.sdf:L201]: XML Element[ros2_control], child of element[model], not defined in SDF. Copying[ros2_control] as children of [model].
[rviz2-4] MESA: error: ZINK: failed to choose pdev
[rviz2-4] glx: failed to create drisw screen
[ruby $(which gz) sim-1] MESA: error: ZINK: failed to choose pdev
[ruby $(which gz) sim-1] glx: failed to create drisw screen
[parameter_bridge-2] [INFO] [1727185583.118929178] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/clock (gz.msgs.Clock) -> /clock (rosgraph_msgs/msg/Clock)] (Lazy 0)
[parameter_bridge-2] [INFO] [1727185583.233970251] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/model/my_robot/odometry (gz.msgs.Odometry) -> /my_robot/odometry (nav_msgs/msg/Odometry)] (Lazy 0)
[parameter_bridge-2] [INFO] [1727185583.249458687] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/world/empty/model/my_robot/joint_state (gz.msgs.Model) -> /joint_states (sensor_msgs/msg/JointState)] (Lazy 0)
[parameter_bridge-2] [INFO] [1727185583.593666224] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/model/my_robot/pose (gz.msgs.Pose_V) -> /tf (tf2_msgs/msg/TFMessage)] (Lazy 0)
[parameter_bridge-2] [INFO] [1727185584.006242993] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/model/my_robot/pose_static (gz.msgs.Pose_V) -> /tf_static (tf2_msgs/msg/TFMessage)] (Lazy 0)
[ruby $(which gz) sim-1] [INFO] [1727185584.724228951] [gz_ros_control]: [gz_ros2_control] Fixed joint [chassis_joint] (Entity=22)] is skipped
[ruby $(which gz) sim-1] [INFO] [1727185584.844514226] [gz_ros_control]: Loading controller_manager
[ruby $(which gz) sim-1] [INFO] [1727185584.934739043] [controller_manager]: Subscribing to '/robot_description' topic for robot description.
[ruby $(which gz) sim-1] [WARN] [1727185584.936191766] [gz_ros_control]: Waiting RM to load and initialize hardware...
[ruby $(which gz) sim-1] [INFO] [1727185584.945986937] [controller_manager]: Received robot description from topic.
[ruby $(which gz) sim-1] terminate called after throwing an instance of 'std::runtime_error'
[ruby $(which gz) sim-1]   what():  the robot tag is not root element in URDF
[ruby $(which gz) sim-1] Stack trace (most recent call last) in thread 36960:
[ruby $(which gz) sim-1] #18   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
[ruby $(which gz) sim-1] #17   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fa9ff4f0c3b, in 
[ruby $(which gz) sim-1] #16   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fa9ff463a93, in 
[ruby $(which gz) sim-1] #15   Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7fa9fa68fbb3, in 
[ruby $(which gz) sim-1] #14   Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7fa9cf839df6, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[ruby $(which gz) sim-1] #13   Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7fa9cf828bf9, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[ruby $(which gz) sim-1] #12   Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7fa9cf8284ca, in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>)
[ruby $(which gz) sim-1] #11   Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7fa9cfb86794, in 
[ruby $(which gz) sim-1] #10   Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7fa9cfb1604a, in controller_manager::ControllerManager::robot_description_callback(std_msgs::msg::String_<std::allocator<void> > const&)
[ruby $(which gz) sim-1] #9    Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7fa9cfb1b85d, in controller_manager::ControllerManager::init_resource_manager(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[ruby $(which gz) sim-1] #8    Object "/opt/ros/jazzy/lib/libgz_ros2_control-system.so", at 0x7fa9cfbfc1e9, in 
[ruby $(which gz) sim-1] #7    Object "/opt/ros/jazzy/lib/libhardware_interface.so", at 0x7fa9cfa084f8, in 
[ruby $(which gz) sim-1] #6    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7fa9fa660127, in __cxa_throw
[ruby $(which gz) sim-1] #5    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7fa9fa64aa48, in std::terminate()
[ruby $(which gz) sim-1] #4    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7fa9fa65fe9b, in 
[ruby $(which gz) sim-1] #3    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7fa9fa64affd, in 
[ruby $(which gz) sim-1] #2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fa9ff3ef8fe, in abort
[ruby $(which gz) sim-1] #1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fa9ff40c26d, in gsignal
[ruby $(which gz) sim-1] #0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fa9ff465b1c, in pthread_kill
[ruby $(which gz) sim-1] Aborted (Signal sent by tkill() 36880 1002)
[rviz2-4] MESA: error: ZINK: failed to choose pdev
[rviz2-4] glx: failed to create drisw screen
[INFO] [ruby $(which gz) sim-1]: process has finished cleanly [pid 36843]
[INFO] [launch]: process[ruby $(which gz) sim-1] was required: shutting down launched system
[INFO] [rviz2-4]: sending signal 'SIGINT' to process[rviz2-4]
[INFO] [robot_state_publisher-3]: sending signal 'SIGINT' to process[robot_state_publisher-3]
[INFO] [parameter_bridge-2]: sending signal 'SIGINT' to process[parameter_bridge-2]
[robot_state_publisher-3] [INFO] [1727185588.125109152] [rclcpp]: signal_handler(signum=2)
[rviz2-4] [INFO] [1727185588.126160310] [rclcpp]: signal_handler(signum=2)
[parameter_bridge-2] [INFO] [1727185588.129134194] [rclcpp]: signal_handler(signum=2)
[rviz2-4] D3D12: Removing Device.
[INFO] [parameter_bridge-2]: process has finished cleanly [pid 36844]
[rviz2-4] [INFO] [1727185588.271469924] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [INFO] [1727185588.271596450] [rviz2]: OpenGl version: 4.2 (GLSL 4.2)
[INFO] [robot_state_publisher-3]: process has finished cleanly [pid 36846]
[rviz2-4] [INFO] [1727185588.720986029] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[rviz2-4]   what():  failed to create guard condition: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/guard_condition.c:67
[ERROR] [rviz2-4]: process has died [pid 36847, exit code -6, cmd '/opt/ros/jazzy/lib/rviz2/rviz2 -d /home/amronos/ros2_ws/install/my_robot/share/my_robot/config/simulation.rviz --ros-args -r __node:=rviz2'].

The SDF file:

<?xml version="1.0" ?>
<sdf version="1.8">
  <model canonical_link="base_link" name="my_robot">
    <!-- BASE -->
    <link name="base_link">
      <must_be_base_link>true</must_be_base_link>
    </link>
    <!-- CHASSIS -->
    <joint name="chassis_joint" type="fixed">
      <parent>base_link</parent>
      <child>chassis_link</child>
      <pose relative_to="base_link">0 0 0.075 0 0 0</pose>
    </joint>
    <link name="chassis_link">
      <pose relative_to="chassis_joint"/>
      <visual name="chassis_link_visual">
        <geometry>
          <box>
            <size>
                0.3 0.3 0.15
              </size>
          </box>
        </geometry>
        <material>
          <ambient>1 1 1 1</ambient>
          <diffuse>1 1 1 1</diffuse>
        </material>
      </visual>
      <collision name="chassis_link_collision">
        <geometry>
          <box>
            <size>
                0.3 0.3 0.15
              </size>
          </box>
        </geometry>
      </collision>
      <inertial>
        <mass>0.5</mass>
        <inertia>
          <ixx>0.0046875</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.0046875</iyy>
          <iyz>0.0</iyz>
          <izz>0.0075</izz>
        </inertia>
      </inertial>
    </link>
    <!-- lEFT WHEEL -->
    <joint name="left_wheel_joint" type="revolute">
      <parent>chassis_link</parent>
      <child>left_wheel_link</child>
      <pose relative_to="chassis_link">0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>
    <link name="left_wheel_link">
      <pose relative_to="left_wheel_joint"/>
      <visual name="left_wheel_link_visual">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0 0 1</ambient>
          <diffuse>0 0 1</diffuse>
        </material>
      </visual>
      <collision name="left_wheel_link_collision">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
      </collision>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>7.583333333333335e-05</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>7.583333333333335e-05</iyy>
          <iyz>0.0</iyz>
          <izz>0.00012500000000000003</izz>
        </inertia>
      </inertial>
    </link>
    <!-- RIGHT WHEEL -->
    <joint name="right_wheel_joint" type="revolute">
      <parent>chassis_link</parent>
      <child>right_wheel_link</child>
      <pose relative_to="chassis_link">0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <!-- limit would not be specified because if the type was continuous -->
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>
    <link name="right_wheel_link">
      <pose relative_to="right_wheel_joint"/>
      <visual name="right_wheel_link_visual">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0 0 1</ambient>
          <diffuse>0 0 1</diffuse>
        </material>
      </visual>
      <collision name="right_wheel_link_collision">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
      </collision>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>7.583333333333335e-05</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>7.583333333333335e-05</iyy>
          <iyz>0.0</iyz>
          <izz>0.00012500000000000003</izz>
        </inertia>
      </inertial>
    </link>
    <!-- CASTER -->
    <joint name="caster_joint" type="revolute">
      <parent>chassis_link</parent>
      <child>caster_link</child>
      <pose relative_to="chassis_link">-0.09 0 -0.075 0 0 0</pose>
      <axis>
        <xyz>1 1 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>
    <link name="caster_link">
      <pose relative_to="caster_joint"/>
      <visual name="caster_link_visual">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0 0 1</ambient>
          <diffuse>0 0 1</diffuse>
        </material>
      </visual>
      <collision name="caster_link_collision">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
      </collision>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.00010000000000000002</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.00010000000000000002</iyy>
          <iyz>0.0</iyz>
          <izz>0.00010000000000000002</izz>
        </inertia>
      </inertial>
    </link>
    <ros2_control name="GazeboSimSystem" type="system">
      <hardware>
        <plugin>gz_ros2_control/GazeboSimSystem</plugin>
      </hardware>
      <joint name="left_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-10</param>
          <param name="max">10</param>
        </command_interface>
        <state_interface name="velocity"/>
        <state_interface name="position"/>
      </joint>
      <joint name="right_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-10</param>
          <param name="max">10</param>
        </command_interface>
        <state_interface name="velocity"/>
        <state_interface name="position"/>
      </joint>
    </ros2_control>
    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
      <parameters>/home/amronos/ros2_ws/install/my_robot/share/my_robot/config/my_controllers.yaml</parameters>
    </plugin>
  </model>
</sdf>

Originally reported at https://github.com/ros/sdformat_urdf/issues/35

christophfroehlich commented 2 weeks ago

ros2_control does not support SDF (yet). If you want to contribute, see https://github.com/ros-controls/ros2_control/issues/632