<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
    <!-- Adding other macros -->
    <xacro:include filename="$(find pointgrey_camera_description)/urdf/pointgrey_flea3.urdf.xacro"/>
    <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro"/>

    <xacro:macro name="atlascar2" params="length width wheel_radius height">
        <!-- Links: inertial,visual,collision -->
        <link name="base_link">
            <inertial>
                <!-- origin is relative -->
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <mass value="2"/>
                <inertia ixx="1" ixy="0" ixz="0"
                        iyy="1" iyz="0"
                        izz="1"/>
            </inertial>
            <visual>
                <origin xyz="${length/2} ${-width/2-0.15} ${-wheel_radius+0.05}" rpy="0 0 ${90*deg_to_rad}"/>
                <geometry>
                    <mesh filename="file:///$(find atlascar2_description)/meshes/atlascar2_nowheels.dae"/>
                </geometry>
            </visual>
            <collision>
                <!-- origin is relative -->
                <origin xyz=" 0 0 ${-wheel_radius + height/2}"/>
                <geometry>
                    <box size="${length - 0.1} ${width} ${height/2}"/>
                </geometry>
            </collision>
        </link>

        <link name="top_collision">
            <collision>
                <origin xyz="${-length/7} 0 0"/>
                <geometry>
                    <box size="${length/2} ${width} ${height/2}"/>
                </geometry>
            </collision>
        </link>

        <joint name="base_link_to_top_collision" type="fixed">
            <origin xyz="0 0 ${height/2}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="top_collision"/>
        </joint>


        <link name="base_footprint">
            <collision>
                <origin xyz="0 0 0"/>
                <geometry>
                    <sphere radius="0.00000001"/>
                </geometry>
            </collision>
        </link>
        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
            <child link="base_link"/>
            <parent link="base_footprint"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="pointgrey_flea3_joints" params="name frame parent x y z rol pit yaw">
        <xacro:pointgrey_flea3 name="${name}" frame="${frame}"/>
        <joint name="{parent}_to_${frame}" type="fixed">
            <origin xyz="${x} ${y} ${z}" rpy="${rol} ${pit} ${yaw}"/>
            <parent link="${parent}"/>
            <child link="${frame}"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="sick_lms1xx_joints" params="frame parent x y z rol pit yaw topic">
        <xacro:sick_lms1xx frame="${frame}" topic="${topic}"/>
        <joint name="{parent}_to_${frame}" type="fixed">
            <origin xyz="${x} ${y} ${z}" rpy="${rol} ${pit} ${yaw}"/>
            <parent link="${parent}"/>
            <child link="${frame}"/>
        </joint>
    </xacro:macro>
</robot>