<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">  
    <gazebo>
        <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
            <parameters>$(find sherpa)/config/ackermann_controller.yaml</parameters>
        </plugin>
    </gazebo>

    <link name="base_link">
        <inertial>
            <mass value="15"/>
            <inertia 
                ixx="0.001828"
                ixy="0.0"
                ixz="0.0"
                iyy="0.001828"
                iyz="0.0"
                izz="0.003528"
            />
        </inertial>
    </link>

    <xacro:include filename="components/camera.xacro"/>
    <xacro:include filename="components/imu.xacro"/>
    <xacro:include filename="components/wheel.xacro"/>
    <xacro:include filename="components/ouster_lidar.xacro"/>

    <xacro:camera name="front_camera" parent="base" xyz="0 0 0.75" rpy="0 0 0" frequency="30" fov="90" topic="/front_camera"/>
    <xacro:imu name="imu" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" frequency="300" topic="/imu"/>
    <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" model="vlp16" frequency="10" topic="/velodyne_points"/>

    <link name="steer_fl_link">
        <inertial>
            <mass value="0.1"/>
            <inertia 
                ixx="0.0"
                ixy="0.0"
                ixz="0.0"
                iyy="0.0"
                iyz="0.0"
                izz="0.0"
            />
        </inertial>
    </link>
    <link name="steer_fr_link">
    <inertial>
            <mass value="0.1"/>
            <inertia 
                ixx="0.0"
                ixy="0.0"
                ixz="0.0"
                iyy="0.0"
                iyz="0.0"
                izz="0.0"
            />
        </inertial>
    </link>
    
    <xacro:wheel name="wheel_fl" parent="steer_fl" xyz="0  0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
    <xacro:wheel name="wheel_fr" parent="steer_fr" xyz="0 -0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
    <xacro:wheel name="wheel_bl" parent="base" xyz="-0.3  0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
    <xacro:wheel name="wheel_br" parent="base" xyz="-0.3 -0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>    

    <joint name="steer_fl_joint" type="revolute">
        <parent link="base_link"/>
        <child link="steer_fl_link"/>
        <origin xyz="0.3 0.20 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
    </joint>

    <joint name="steer_fr_joint" type="revolute">
        <parent link="base_link"/>
        <child link="steer_fr_link"/>
        <origin xyz="0.3 -0.20 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
    </joint>

    
    <ros2_control name="SherpaSystem" type="system">
        <hardware>
            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </hardware>
    
        <joint name="wheel_bl_joint">
            <command_interface name="velocity"/>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>

        <joint name="wheel_br_joint">
            <command_interface name="velocity"/>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>

        <joint name="steer_fl_joint">
            <command_interface name="position"/>
            <state_interface name="position"/>
        </joint>

        <joint name="steer_fr_joint">
            <command_interface name="position"/>
            <state_interface name="position"/>
        </joint>
    </ros2_control>
</robot>
