<?xml version="1.0" ?>
<robot name="larcc" xmlns:xacro="http://ros.org/wiki/xacro">

    <!-- ####################################### -->
    <!-- Arguments and propperties-->
    <!-- ####################################### -->
    <!--Args are local, properties are global, so we copy the values of the arguments to the properties-->
    <!--Properties are global so they can be viewed from inside the included xacros-->
    <!--  args are used with $(arg arg_name), properties are used as ${property_name}-->

    <xacro:property name="M_PI" value="3.1415926535897931"/>

    <!-- Tables properties-->
    <xacro:property name="table_height" value="0.795"/>
    <xacro:property name="table_width" value=".9"/>
    <xacro:property name="table_length" value="1.25"/>
    <xacro:property name="table2_height" value="0.75"/>
    <xacro:property name="table2_width" value=".7"/>
    <xacro:property name="table2_length" value="1.20"/>
    <xacro:property name="table_tabletop_thickness" value="0.015"/>
    <xacro:property name="table_leg_radius" value="0.02"/>

    <!-- Gantry properties-->
    <xacro:property name="gantry_height" value="2.29"/>
    <xacro:property name="gantry_width" value="4"/>
    <xacro:property name="gantry_length" value="2.8"/>
    <xacro:property name="gantry_thickness" value="0"/>
    <xacro:property name="gantry_leg_radius" value="0.09"/>

    <!-- Manipulator's args and propperties -->
<!--    <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/ur10e/joint_limits.yaml"/>-->
<!--    <xacro:arg name="physical_params" default="$(find ur_description)/config/ur10e/physical_parameters.yaml"/>-->
<!--    <xacro:arg name="kinematics_params" default="$(find ur_description)/config/ur10e/default_kinematics.yaml"/>-->
<!--    <xacro:arg name="visual_params" default="$(find ur_description)/config/ur10e/visual_parameters.yaml"/>-->

    <!--Parameter files -->
    <!--  <arg name="joint_limit_params" default="$(find ur_description)/config/ur10e/joint_limits.yaml"/>-->
    <!--  <arg name="kinematics_params" default="$(find ur_description)/config/ur10e/default_kinematics.yaml"/>-->
    <!--  <arg name="physical_params" default="$(find ur_description)/config/ur10e/physical_parameters.yaml"/>-->
    <!--  <arg name="visual_params" default="$(find ur_description)/config/ur10e/visual_parameters.yaml"/>-->
    <!--
      legal values:
        - hardware_interface/PositionJointInterface
        - hardware_interface/VelocityJointInterface
        - hardware_interface/EffortJointInterface

      NOTE: this value must correspond to the controller configured in the
            controller .yaml files in the 'config' directory.
    -->
<!--    <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>-->
<!--    <xacro:arg name="safety_limits" default="false"/>-->
<!--    <xacro:arg name="safety_pos_margin" default="0.15"/>-->
<!--    <xacro:arg name="safety_k_position" default="20"/>-->

    <!-- ####################################### -->
    <!-- Include files-->
    <!-- ####################################### -->

    <!-- Scene objets-->
    <xacro:include filename="$(find larcc_description)/urdf/properties.xacro"/>
    <xacro:include filename="$(find larcc_description)/urdf/table.macro.xacro"/>
    <xacro:include filename="$(find larcc_description)/urdf/gantry.macro.xacro"/>
    <xacro:include filename="$(find larcc_description)/urdf/tripod.macro.xacro"/>
<!--    <xacro:include filename="$(find robotiq_2f_140_gripper_visualization)/urdf/robotiq_arg2f_140_model_macro.xacro" />-->


    <!-- Sensors-->
    <xacro:include filename="$(find larcc_description)/urdf/astra.urdf.xacro"/>
    <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>

    <xacro:include filename="$(find hector_xacro_tools)/urdf/inertia_tensors.urdf.xacro"/>
    <xacro:include filename="$(find hector_xacro_tools)/urdf/joint_macros.urdf.xacro"/>
    <xacro:include filename="$(find larcc_description)/urdf/kinect_larcc.urdf.xacro"/>

    <!-- Manipulator-->
<!--    <xacro:include filename="$(find ur_gazebo)/urdf/ur_macro.xacro"/>-->

    <!-- ####################################### -->
    <!-- Declare links and joints -->
    <!-- ####################################### -->
    <link name="world"/>

    <!-- Place table 1 in the scene-->
    <xacro:table_macro name="table" parent="world" width="${table_width}"
                       length="${table_length}" height="${table_height}"
                       tabletop_thickness="${table_tabletop_thickness}" leg_radius="${table_leg_radius}">
        <origin xyz="0.1 ${gantry_length/2-table_width/2} 0" rpy="0 0 ${-M_PI/2}"/>
    </xacro:table_macro>

    <!-- Place table 2 in the scene-->
    <!-- <xacro:table_macro name="table2" parent="world" width="${table2_width}"
                       length="${table2_length}" height="${table2_height}"
                       tabletop_thickness="${table_tabletop_thickness}" leg_radius="${table_leg_radius}">
        <origin xyz="0.1 ${gantry_length/2-table2_width/2-table_width} 0" rpy="0 0 ${-M_PI/2}"/>
    </xacro:table_macro> -->

    <!-- Place grantry 1 in the scene-->
    <xacro:gantry_macro name="gantry" parent="world" width="${gantry_width}"
                        length="${gantry_length}" height="${gantry_height}"
                        gantry_thickness="${gantry_thickness}" leg_radius="${gantry_leg_radius}">
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </xacro:gantry_macro>

    <!--      Manipulator-->
<!--    <xacro:ur_robot_gazebo-->
<!--            prefix=""-->
<!--            joint_limits_parameters_file="$(arg joint_limit_params)"-->
<!--            kinematics_parameters_file="$(arg kinematics_params)"-->
<!--            physical_parameters_file="$(arg physical_params)"-->
<!--            visual_parameters_file="$(arg visual_params)"-->
<!--            transmission_hw_interface="$(arg transmission_hw_interface)"-->
<!--            safety_limits="$(arg safety_limits)"-->
<!--            safety_pos_margin="$(arg safety_pos_margin)"-->
<!--            safety_k_position="$(arg safety_k_position)"/>-->


    <!--    Place gripper-->
    <!-- <xacro:robotiq_arg2f_140 prefix=""/>

    <joint name="new_gripper_joint" type="fixed">
        <parent link="tool0"/>
        <child link="robotiq_arg2f_base_link"/>
        <origin xyz="0 0 0" rpy="0 0 1.5708"/>
    </joint> -->

    
<!--    &lt;!&ndash;  Place robot on top of the table1&ndash;&gt;-->
<!--    <joint name="tabletop_table_to_ur10e" type="fixed">-->
<!--        <parent link="tabletop_table"/>-->
<!--        <child link="base_link"/>-->
<!--        <origin xyz="0.0 0.0 0.001" rpy="0.0 0.0 0.0"/>-->
<!--    </joint>-->

    <xacro:VLP-16 name="lidar_1" parent="rectangularbeam3_gantry" topic="/lidar_1/velodyne_points" gpu="true">
        <origin xyz="1.955777406692505e-08 1.677582025527954 -0.07595625519752502"
                rpy="-1.4068863031624894 0.9304128095436818 -0.8782068835816081"/>
    </xacro:VLP-16>

    <xacro:VLP-16 name="lidar_2" parent="rectangularbeam2_gantry" topic="/lidar_2/velodyne_points" gpu="true">
        <origin xyz="0.055899396538734436 2.29645037651062 -0.02108168601989746"
                rpy="-1.5838383865967978 -0.010889059518626235 -0.9922356621235997"/>
    </xacro:VLP-16>

    <xacro:VLP-16 name="lidar_3" parent="rectangularbeam1_gantry" topic="/lidar_3/velodyne_points" gpu="true">
        <origin xyz="-0.042497698217630386 1.5332698822021484 0.08965662121772766"
                rpy="-1.9820141210538103 0.011203647336123805 -1.0491534231905575"/>
    </xacro:VLP-16>

    <!--    CALIBRATED-->

<!--    <xacro:sensor_astra prefix="hand_camera" parent="flange" scale="0.48 0.66 0.5">-->
<!--        <origin rpy="${M_PI} 0.0 0" xyz="-0.02 -0.06 -0.065"/>-->
<!--    </xacro:sensor_astra>-->


    <xacro:kinect_camera name="camera_1" parent="bigbeam1_gantry">
        <origin xyz="2.8 0 -0.09"
                rpy="0 ${M_PI/6} ${-M_PI/2}"/>
    </xacro:kinect_camera>

    <xacro:sensor_astra prefix="camera_2" parent="smallbeam2_bigbeam1_gantry" scale="1 1 1">
        <origin xyz="-0.0899999737739563 2.5493040084838867 -0.10000000149011612"
                rpy="0.016022853466738745 0.4975036324125581 -2.2477926358273677"/>
    </xacro:sensor_astra>

    <xacro:sensor_astra prefix="camera_3" parent="smallbeam1_bigbeam1_gantry" scale="1 1 1">
        <origin xyz="0.09000001847743988 2.4310083389282227 -0.09000000357627869"
                rpy="-0.07610853648541062 0.5950234751036887 -0.7551412632911726"/>

    </xacro:sensor_astra>

    <xacro:sensor_astra prefix="camera_4" parent="bigbeam1_gantry" scale="1 1 1">
        <origin xyz="2.5 0 -0.09"
                rpy="0 ${M_PI/6} ${-M_PI/2}"/>
    </xacro:sensor_astra>


</robot>