<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
  <xacro:macro name="bot0_ros2_control">
    <!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
    <ros2_control name="Bot0GazeboSystem" type="system">
      <!-- 硬件插件：关联Gazebo仿真 -->
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>

      <!-- 2. 腿部+手臂关节（位置控制接口，对应bot0_position_controller） -->
      <!-- 2.1 腿部关节 -->
      <joint name="left_hip_pitch_joint">
        <command_interface name="position">  <!-- 位置控制器的命令接口 -->
          <param name="min">-1.0</param>    <!-- 对应关节限位（弧度） -->
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />  <!-- 反馈接口：位置 -->
        <state_interface name="velocity" />  <!-- 反馈接口：速度 -->
        <state_interface name="effort" />    <!-- 反馈接口：力（用于调试） -->
      </joint>

      <joint name="left_leg_pitch_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_hip_pitch_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_leg_pitch_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <!-- 2.2 手臂关节 -->
      <joint name="left_shoulder_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="left_arm1_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="left_arm2_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_shoulder_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_arm1_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_arm2_joint">
        <command_interface name="position">
          <param name="min">-1.0</param>
          <param name="max">1.0</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <!-- 3. 车轮关节（速度控制接口，对应bot0_wheel_velocity_controller） -->
      <joint name="left_wheel1_joint">
        <command_interface name="velocity">  <!-- 速度控制器的命令接口 -->
          <param name="min">-50</param>      <!-- 对应关节速度限位（rad/s） -->
          <param name="max">50</param>
        </command_interface>
        <state_interface name="position" />  <!-- 反馈接口：位置 -->
        <state_interface name="velocity" />  <!-- 反馈接口：速度 -->
        <state_interface name="effort" />    <!-- 反馈接口：力 -->
      </joint>

      <joint name="left_wheel2_joint">
        <command_interface name="velocity">
          <param name="min">-50</param>
          <param name="max">50</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_wheel1_joint">
        <command_interface name="velocity">
          <param name="min">-50</param>
          <param name="max">50</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

      <joint name="right_wheel2_joint">
        <command_interface name="velocity">
          <param name="min">-50</param>
          <param name="max">50</param>
        </command_interface>
        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>
    </ros2_control>

    <!-- 4. Gazebo插件配置：关联控制器参数文件 -->
    <gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <!-- 加载控制器配置文件（路径需替换为你的实际路径） -->
        <parameters>$(find hive_core_r0)/config/bot0_ros2_controller.yaml</parameters>
      </plugin>
    </gazebo>
  </xacro:macro>
</robot>
