<?xml version="1.0" ?>
<sdf version="1.8">
    <world name="Moving_robot">
        <physics name="1ms" type="ignored">
            <max_step_size>0.001</max_step_size>
            <real_time_factor>1.0</real_time_factor>
        </physics>
        <plugin
            filename="gz-sim-physics-system"
            name="gz::sim::systems::Physics">
        </plugin>
        <plugin
            filename="gz-sim-user-commands-system"
            name="gz::sim::systems::UserCommands">
        </plugin>
        <plugin
            filename="gz-sim-scene-broadcaster-system"
            name="gz::sim::systems::SceneBroadcaster">
        </plugin>

        <light type="directional" name="sun">
            <cast_shadows>true</cast_shadows>
            <pose>0 0 10 0 0 0</pose>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
            <attenuation>
                <range>1000</range>
                <constant>0.9</constant>
                <linear>0.01</linear>
                <quadratic>0.001</quadratic>
            </attenuation>
            <direction>-0.5 0.1 -0.9</direction>
        </light>

        <model name="ground_plane">
            <static>true</static>
            <link name="link">
                <collision name="collision">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    </plane>
                </geometry>
                </collision>
                <visual name="visual">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    <size>100 100</size>
                    </plane>
                </geometry>
                <material>
                    <ambient>0.8 0.8 0.8 1</ambient>
                    <diffuse>0.8 0.8 0.8 1</diffuse>
                    <specular>0.8 0.8 0.8 1</specular>
                </material>
                </visual>
            </link>
        </model>

        <model name='vehicle_blue' canonical_link='chassis'>
            <pose relative_to='world'>0 0 0 0 0 0</pose>   <!--the pose is relative to the world by default-->

            <link name='chassis'>
                <pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
                <inertial> <!--inertial properties of the link mass, inertia matix-->
                    <mass>1.14395</mass>
                    <inertia>
                        <ixx>0.126164</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.416519</iyy>
                        <iyz>0</iyz>
                        <izz>0.481014</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <box>
                            <size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
                        </box>
                    </geometry>
                    <!--let's add color to our link-->
                    <material>
                        <ambient>0.0 0.0 1.0 1</ambient>
                        <diffuse>0.0 0.0 1.0 1</diffuse>
                        <specular>0.0 0.0 1.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'> <!--todo: describe why we need the collision-->
                    <geometry>
                        <box>
                            <size>2.0 1.0 0.5</size>
                        </box>
                    </geometry>
                </collision>
            </link>

            <!--let's build the left wheel-->
            <link name='left_wheel'>
                <pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
                <inertial>
                    <mass>2</mass>
                    <inertia>
                        <ixx>0.145833</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.145833</iyy>
                        <iyz>0</iyz>
                        <izz>0.125</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                    <material>
                        <ambient>1.0 0.0 0.0 1</ambient>
                        <diffuse>1.0 0.0 0.0 1</diffuse>
                        <specular>1.0 0.0 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                </collision>
            </link>

            <!--copy and paste for right wheel but change position-->
            <link name='right_wheel'>
                <pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
                <inertial>
                    <mass>1</mass>
                    <inertia>
                        <ixx>0.145833</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.145833</iyy>
                        <iyz>0</iyz>
                        <izz>0.125</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                    <material>
                        <ambient>1.0 0.0 0.0 1</ambient>
                        <diffuse>1.0 0.0 0.0 1</diffuse>
                        <specular>1.0 0.0 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                </collision>
            </link>

            <frame name="caster_frame" attached_to='chassis'>
                <pose>0.8 0 -0.2 0 0 0</pose>
            </frame>

            <!--caster wheel-->
            <link name='caster'>
                <pose relative_to='caster_frame'/>
                <inertial>
                    <mass>1</mass>
                    <inertia>
                        <ixx>0.1</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.1</iyy>
                        <iyz>0</iyz>
                        <izz>0.1</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <sphere>
                            <radius>0.2</radius>
                        </sphere>
                    </geometry>
                    <material>
                        <ambient>0.0 1 0.0 1</ambient>
                        <diffuse>0.0 1 0.0 1</diffuse>
                        <specular>0.0 1 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <sphere>
                            <radius>0.2</radius>
                        </sphere>
                    </geometry>
                </collision>
            </link>


            <!--connecting these links together using joints-->
            <joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
                <pose relative_to='left_wheel'/>
                <parent>chassis</parent>
                <child>left_wheel</child>
                <axis>
                    <xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
                    <limit>
                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
                        <upper>1.79769e+308</upper>     <!--positive infinity-->
                    </limit>
                </axis>
            </joint>

            <joint name='right_wheel_joint' type='revolute'>
                <pose relative_to='right_wheel'/>
                <parent>chassis</parent>
                <child>right_wheel</child>
                <axis>
                    <xyz expressed_in='__model__'>0 1 0</xyz>
                    <limit>
                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
                        <upper>1.79769e+308</upper>     <!--positive infinity-->
                    </limit>
                </axis>
            </joint>

            <!--different type of joints ball joint--> <!--defult value is the child-->
            <joint name='caster_wheel' type='ball'>
                <parent>chassis</parent>
                <child>caster</child>
            </joint>

            <!--diff drive plugin-->
            <plugin
                filename="gz-sim-diff-drive-system"
                name="gz::sim::systems::DiffDrive">
                <left_joint>left_wheel_joint</left_joint>
                <right_joint>right_wheel_joint</right_joint>
                <wheel_separation>1.2</wheel_separation>
                <wheel_radius>0.4</wheel_radius>
                <odom_publish_frequency>1</odom_publish_frequency>
                <topic>cmd_vel</topic>
            </plugin>
        </model>

        <!-- Moving Left-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777234</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.0}, angular: {z: 0.5}
            </output>
        </plugin>
        <!-- Moving Forward-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777235</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.5}, angular: {z: 0.0}
            </output>
        </plugin>
        <!-- Moving Right-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777236</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.0}, angular: {z: -0.5}
            </output>
        </plugin>
        <!-- Moving Backward-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777237</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: -0.5}, angular: {z: 0.0}
            </output>
        </plugin>
    </world>
</sdf>