<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find atlascar2_description)/urdf/atlascar2_model_macro.urdf.xacro" />
    <xacro:include filename="$(find pointgrey_camera_description)/urdf/pointgrey_flea3.urdf.xacro" />
    <xacro:include filename="$(find sick_ldmrs_description)/urdf/sick_ldmrs_macros.urdf.xacro" />
    <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro" />
    <xacro:include filename="$(find atlascar2_description)/urdf/asus_xtion.urdf.xacro" />


    <xacro:macro name="atlascar2" params="prefix">
        <!-- links -->
        <link name="${prefix}base_link" />
        <link name="${prefix}base_footprint" />

        <!-- car model -->
        <xacro:atlascar2_model prefix="${prefix}" />

        <!-- joints -->
        <joint name="${prefix}base_link_to_${prefix}base_footprint" type="fixed">
            <origin xyz="0 0 -0.305" rpy="0 0 0" />
            <parent link="${prefix}base_link" />
            <child link="${prefix}base_footprint" />
        </joint>

        <joint name="${prefix}base_footprint_to_${prefix}model" type="fixed">
            <origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
            <parent link="${prefix}base_footprint" />
            <child link="${prefix}model" />
        </joint>

        <!-- top-right-camera -->
        <xacro:pointgrey_flea3 name="${prefix}top_right_camera" frame="${prefix}top_right_camera" />

        <joint name="${prefix}base_link_to_${prefix}top_right_camera" type="fixed">
            <origin xyz="-1.47 -0.2367 1.3423" rpy="-0.007146 0.177238 -0.003179" />
            <!--            <origin xyz="-1.55 -0.22 1.35" rpy="0 0.4 0" />-->
            <parent link="${prefix}base_link" />
            <child link="${prefix}top_right_camera" />
        </joint>

        <!-- top-left-camera -->
        <xacro:pointgrey_flea3 name="${prefix}top_left_camera" frame="${prefix}top_left_camera" />

        <joint name="${prefix}base_link_to_${prefix}top_left_camera" type="fixed">
            <origin xyz="-1.48 0.22 1.35" rpy="0 0.1987204 0" />
<!--            <origin xyz="-1.55 0.22 1.35" rpy="0 0.4 0" />-->
            <parent link="${prefix}base_link" />
            <child link="${prefix}top_left_camera" />
        </joint>


        <!-- laser sensor lms_ldmrs400001 -->
        <xacro:sick_ldmrs name="${prefix}frontal_laser" />

        <joint name="${prefix}base_link_to_${prefix}frontal_laser_mount_link" type="fixed">
            <origin xyz="0 0 0" rpy="0 0 0" />
            <parent link="${prefix}base_link" />
            <child link="${prefix}frontal_laser_mount_link" />
        </joint>


        <!-- left lms151 laser -->
        <xacro:sick_lms1xx frame="${prefix}right_laser" />

        <joint name="${prefix}base_link_to_${prefix}right_laser" type="fixed">
            <origin xyz="0.059 -0.6872 0.2287" rpy="0.046505 -0.027762 -0.742348" />
<!--            <origin xyz="-0.07 -0.76 0.27" rpy="0 0 ${-0.7854}" />-->
            <parent link="${prefix}base_link" />
            <child link="${prefix}right_laser" />
        </joint>

        <!-- right lms151 laser -->
        <xacro:sick_lms1xx frame="${prefix}left_laser" />

        <joint name="${prefix}base_link_to_${prefix}left_laser" type="fixed">
            <origin xyz="-0.0415 0.8444 0.3349" rpy="0.001728 -0.012468 0.888841" />
<!--            <origin xyz="-0.07 0.76  0.27" rpy="0 0 ${0.7854}" />-->
            <parent link="${prefix}base_link" />
            <child link="${prefix}left_laser" />
        </joint>

        <!-- RGB-D camera -->
        <xacro:asus_camera_model name="${prefix}top_center_rgbd_camera" />

        <joint name="${prefix}base_link_to_${prefix}top_center_rgbd_camera" type="fixed">


<!--            <origin xyz="-1.510 -0.010 1.397" rpy="-0.000 0.223 -0.000" />-->
                <origin xyz="-1.510 -0.010 1.338" rpy="-0.000 0.207 -0.000" />
<!--            <origin xyz="-1.51 -0.01 1.27" rpy="0 0.4 0" />-->
            <parent link="${prefix}base_link" />
            <child link="${prefix}top_center_rgbd_camera_link" />
        </joint>




    </xacro:macro>
</robot>