<link name="base_footprint">
    <origin xyz="0 0 0" rpy="0 0 0" />
</link>


<joint name="base_joint" type="fixed">
    <parent link="base_footprint" />
    <child link="base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</joint>

<link name="base_link">
    <visual>
        <origin xyz="0.30 0 0.25" rpy="0 0 0" />
        <geometry>
            <box size="0.68 0.46 0.10"/>
        </geometry>
        <material name="white">
            <color rgba="1 1 1 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0.30 0 0.25" rpy="0 0 0" />
        <geometry>
            <box size="0.68 0.46 0.10"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="9"/>
        <inertia ixx="0.3543" ixy="0" ixz="0" iyy="0.5055" iyz="0" izz="0.1662"/>
    </inertial>
</link> 

<!-- 后轮：驱动功能（continuous关节） -->
<joint name="rear_left_wheel_joint" type="continuous">
    <origin xyz="0 0.3 0.10" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0 1 0"/>
</joint>
<link name="left_wheel_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.2"/>
        <inertia ixx="0.0005139" ixy="0" ixz="0" iyy="0.0005139" iyz="0" izz="0.001"/>
    </inertial>
</link>

<joint name="rear_right_wheel_joint" type="continuous">
    <origin xyz="0 -0.3 0.10" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0 1 0"/>
</joint>
<link name="right_wheel_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.2"/>
        <inertia ixx="0.0005139" ixy="0" ixz="0" iyy="0.0005139" iyz="0" izz="0.001"/>
    </inertial>
</link>

<!-- 左前轮：阿克曼转向结构 -->
<joint name="front_left_steer_joint" type="revolute">
    <origin xyz="0.595 0.20 0.10" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="left_front_steer_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.5" upper="0.5" effort="10" velocity="1.0"/>
</joint>

<link name="left_front_steer_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.1"/>
            <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
</link>


<joint name="front_left_wheel_joint" type="continuous">
    <parent link="left_front_steer_link"/>
    <child link="left_front_link"/>
    <origin rpy="0 0 0" xyz="0 0.1 0"/>
    <axis xyz="0 1 0"/>
</joint>

<link name="left_front_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.2"/>
            <inertia ixx="0.0005139" ixy="0" ixz="0" iyy="0.0005139" iyz="0" izz="0.001"/>
    </inertial>
</link>

<!-- 右前轮：与左前轮结构完全一致 -->
<joint name="front_right_steer_joint" type="revolute">
    <origin xyz="0.595 -0.20 0.10" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="right_front_steer_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.5" upper="0.5" effort="10" velocity="1.0"/>
</joint>

<link name="right_front_steer_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.1"/>
            <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
</link>

<joint name="front_right_wheel_joint" type="continuous">
    <origin xyz="0 -0.1 0" rpy="0 0 0"/>
    <parent link="right_front_steer_link"/>
    <child link="right_front_link"/>
    <axis xyz="0 1 0"/>
</joint>

<link name="right_front_link">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.10" length="0.04"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.2"/>
        <inertia ixx="0.0005139" ixy="0" ixz="0" iyy="0.0005139" iyz="0" izz="0.001"/>
    </inertial>
</link>

<link name="virtual_steer_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02" />
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
    <inertial>  <!-- Gazebo won't show this link without this inertial mass -->
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <mass value="0.1" />
            <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
    <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <box size="0.01 0.15 0.05" />
        </geometry>
    </collision>

</link>

<joint name="virtual_steering_wheel_joint" type="revolute">
    <origin xyz="0.59 0 0.3" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="virtual_steer_link" />
    <axis xyz="1 0 0" />
    <limit lower="-3" upper="3" effort="200" velocity="20" />
</joint>


<!-- IMU -->
<joint name="imu_joint" type="fixed">
    <origin xyz="0.3 0 0.3" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="imu_link"/>
</joint>
<link name="imu_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual> 
    <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <box size="0.02 0.02 0.02"/>
        </geometry>
    </collision>
    <inertial>
        <mass value="0.1"/>
        <inertia ixx="0.0001904" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>      
</link> 

<gazebo>
    <plugin name="gazebo_ros_joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
        <update_rate>100</update_rate>
        <joint_name>front_left_wheel_joint</joint_name>
        <joint_name>front_right_wheel_joint</joint_name>
        <joint_name>rear_left_wheel_joint</joint_name>
        <joint_name>rear_right_wheel_joint</joint_name>
        <joint_name>front_left_steer_joint</joint_name>
        <joint_name>front_right_steer_joint</joint_name>
        <joint_name>virtual_steering_wheel_joint</joint_name>
    </plugin>
</gazebo>

<gazebo>
    <plugin name="gazebo_ros_joint_pose_trajectory"
        filename="libgazebo_ros_joint_pose_trajectory.so">
        <update_rate>2</update_rate>
    </plugin>
</gazebo>

<gazebo>
    <plugin name="ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">
        <ros>
            <remapping>cmd_vel:=cmd_vel</remapping>
            <remapping>odom:=odom</remapping>
            <remapping>distance:=distance</remapping>

        </ros>
        <update_rate>100.0</update_rate>
        
        <front_left_joint>front_left_wheel_joint</front_left_joint>
        <front_right_joint>front_right_wheel_joint</front_right_joint>
        <rear_left_joint>rear_left_wheel_joint</rear_left_joint>
        <rear_right_joint>rear_right_wheel_joint</rear_right_joint>
        <left_steering_joint>front_left_steer_joint</left_steering_joint>
        <right_steering_joint>front_right_steer_joint</right_steering_joint>
        <steering_wheel_joint>virtual_steering_wheel_joint</steering_wheel_joint>
        
        
        <left_steering_pid_gain>1500 0 1</left_steering_pid_gain>
        <left_steering_i_range>0 0</left_steering_i_range>
        <right_steering_pid_gain>1500 0 1</right_steering_pid_gain>
        <right_steering_i_range>0 0</right_steering_i_range>
        <linear_velocity_pid_gain>1000 0 1</linear_velocity_pid_gain>
        <linear_velocity_i_range>0 0</linear_velocity_i_range>

        <max_steer>0.5</max_steer>
        <max_steering_angle>7.85</max_steering_angle>
        <max_speed>5</max_speed>
        
        <publish_odom>true</publish_odom>
        <publish_odom_tf>true</publish_odom_tf> 
        <publish_wheel_tf>false</publish_wheel_tf>
        
        <odometry_frame>odom</odometry_frame>
        <robot_base_frame>base_link</robot_base_frame>
    </plugin>
</gazebo>

</robot>