<?xml version="1.0"?>

<robot name="b2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:arg name="DEBUG" default="false"/>
  <xacro:arg name="GAZEBO" default="false"/>

  <xacro:include filename="$(find b2_description)/xacro/const.xacro"/>
  <xacro:include filename="$(find b2_description)/xacro/materials.xacro"/>
  <xacro:include filename="$(find b2_description)/xacro/leg.xacro"/>

  <xacro:if value="$(arg GAZEBO)">
    <xacro:include filename="$(find b2_description)/xacro/gazebo.xacro"/>
  </xacro:if>
  <xacro:unless value="$(arg GAZEBO)">
    <xacro:include filename="$(find b2_description)/xacro/ros2_control.xacro"/>
  </xacro:unless>


  <!-- Rotor related joint and link is only for demonstrate location. -->
  <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->

  <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
  <xacro:if value="$(arg DEBUG)">
    <link name="world"/>
    <joint name="base_static_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <parent link="world"/>
      <child link="base"/>
    </joint>
  </xacro:if>

  <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>

  <joint name="floating_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base"/>
    <child link="trunk"/>
  </joint>

  <link name="trunk">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find b2_description)/meshes/trunk.dae" scale="1 1 1"/>
      </geometry>
      <!-- <material name="orange"/> -->
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
      </geometry>
    </collision>
    <!-- <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
            <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
        </geometry>
    </collision> -->
    <inertial>
      <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
      <mass value="${trunk_mass}"/>
      <inertia
          ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
          iyy="${trunk_iyy}" iyz="${trunk_iyz}"
          izz="${trunk_izz}"/>
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="trunk"/>
    <child link="imu_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <link name="imu_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
      <!-- <material name="red"/> -->
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
  </link>

  <joint name="head_joint" type="fixed">
    <parent link="trunk"/>
    <child link="head_Link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <link name="head_Link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.33989 -0.000168 0.12029"/>
      <mass value="3.366"/>
      <inertia
          ixx="0.026455"
          ixy="3.15E-05"
          ixz="0.00166"
          iyy="0.029221"
          iyz="-3.27E-05"
          izz="0.010918"/>
    </inertial>
    <collision>
      <origin rpy="0 0 0" xyz="0.41 0 0.005"/>
      <geometry>
        <box size="0.04 0.12 0.14"/>
      </geometry>
    </collision>
  </link>

  <joint name="lidar_joint" type="fixed">
    <parent link="trunk"/>
    <child link="lidar_link"/>
    <origin rpy="0 0 0" xyz="0.34218 0 0.17851"/>
  </joint>

  <link name="lidar_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="6.55E-08"/>
      <inertia
          ixx="1.64E-15"
          ixy="0"
          ixz="-1.09E-47"
          iyy="1.64E-15"
          iyz="-1.32E-47"
          izz="1.64E-15"/>
    </inertial>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
      <geometry>
        <cylinder length="0.16" radius="0.076"/>
      </geometry>
    </collision>
  </link>

  <joint name="tail_joint" type="fixed">
    <parent link="trunk"/>
    <child link="tail_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <link name="tail_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.37448 0.000488 0.006495"/>
      <mass value="0.777"/>
      <inertia
          ixx="0.0028118"
          ixy="1.95E-05"
          ixz="-7.87E-06"
          iyy="0.0047506"
          iyz="-1.57E-05"
          izz="0.0029154"/>
    </inertial>
    <collision>
      <origin rpy="0 0 0" xyz="-0.405 0 0.005"/>
      <geometry>
        <box size="0.025 0.12 0.14"/>
      </geometry>
    </collision>
  </link>


  <xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True"/>
  <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True"/>
  <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False"/>
  <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False"/>

  <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->

</robot>
