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

    <xacro:arg name="ROBOT" default="cyber_dog"/>
    <xacro:arg name="USE_LIDAR" default="false"/>
    <xacro:include filename="const.xacro"/>
    <xacro:include filename="leg.xacro"/>
    <xacro:include filename="gazebo.xacro"/>
    <xacro:include filename="ros2_control.xacro"/>

    <link name="base_link">
        <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_link"/>
        <child link="body"/>
    </joint>

    <link name="body">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="file://$(find cyberdog_description)/meshes/body.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="${body_length} ${body_width} ${body_height}"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}"/>
            <mass value="${body_mass}"/>
            <inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}"
                     izz="${body_izz}"/>
        </inertial>
    </link>

    <joint name="imu_joint" type="fixed">
        <parent link="body"/>
        <child link="imu_link"/>
        <origin rpy="0 0 0" xyz="33.4e-3 -17.2765e-3 51.0469e-3"/>
    </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>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size=".001 .001 .001"/>
            </geometry>
        </collision>
    </link>

    <joint name="scan_joint" type="fixed">
        <parent link="body"/>
        <child link="lidar_link"/>
        <origin rpy="0 0 0" xyz="0.21425 0 0.0908"/>
    </joint>

    <link name="lidar_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>
    </link>

    <joint name="D435_camera_joint" type="fixed">
        <parent link="body"/>
        <child link="D435_camera_link"/>
        <origin rpy="0 0 0" xyz="271.994e-3 25e-3 114.912e-3"/>
    </joint>

    <link name="D435_camera_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>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size=".001 .001 .001"/>
            </geometry>
        </collision>
    </link>

    <joint name="RGB_camera_joint" type="fixed">
        <parent link="body"/>
        <child link="RGB_camera_link"/>
        <origin rpy="0 0 0" xyz="275.76e-3 0 125.794e-3"/>
    </joint>

    <link name="RGB_camera_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>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size=".001 .001 .001"/>
            </geometry>
        </collision>
    </link>

    <joint name="AI_camera_joint" type="fixed">
        <parent link="body"/>
        <child link="AI_camera_link"/>
        <origin rpy="0 0 0" xyz="290.228e-3 0 147.420e-3"/>
    </joint>

    <link name="AI_camera_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>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size=".001 .001 .001"/>
            </geometry>
        </collision>
    </link>

    <xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0"/>
    </xacro:leg>

    <!-- This link is only for head collision -->
    <joint name="head_joint" type="fixed">
        <parent link="body"/>
        <child link="head"/>
        <origin rpy="0 0 0" xyz="0 0 0"/>
    </joint>
    <link name="head">
        <collision>
            <origin rpy="0 0 0" xyz="0.256 0 0.120"/>
            <geometry>
                <box size="0.076 0.060 0.040"/>
            </geometry>
        </collision>
        <collision>
            <origin rpy="0 0 0" xyz="0.225 0 0.150"/>
            <geometry>
                <box size="0.020 0.080 0.100"/>
            </geometry>
        </collision>
    </link>

    <gazebo reference="body">
        <visual>
            <material>
                <ambient>.1 .1 .1 1.0</ambient>
                <diffuse>.1 .1 .1 1.0</diffuse>
                <specular>.1 .1 .1 1.0</specular>
            </material>
        </visual>
    </gazebo>

</robot>