<?xml version="1.0"?>

<!--
 * Copyright (C) 2020 Ghost Robotics - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
-->

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="vision60_composite">
    <xacro:include filename="$(find gr_vision60)/urdf/vision60_3.8_macro.urdf.xacro"/>
    <xacro:include filename="$(find gr_vision60)/urdf/vision60_v4_t265_2pmd_sensorhead.urdf.xacro"/>

    <xacro:vision60/>
    <xacro:sensorhead/>

    <!-- IMU -->
    <link name="imu_link"/>
    <joint name="body_to_imu_link" type="fixed">
        <parent link="body"/>
        <child link="imu_link"/>
        <origin xyz="0 0.3 0.15" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
    </joint>

    <!-- LiDar -->
    <link name="velodyne"/>
    <joint name="body_to_velodyne" type="fixed">
        <parent link="body"/>
        <child link="velodyne"/>
        <origin xyz="0.3 0 0.15" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
    </joint>

    <!-- Ouster -->
    <!-- <link name="os1_sensor"/>
    <joint name="body_to_os1_sensor" type="fixed">
        <parent link="body"/>
        <child link="os1_sensor"/>
        <origin xyz="0.4372 0 0" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
    </joint> -->

    <!-- ToF-->
    <joint name="body_to_t265_pose_frame" type="fixed">
        <parent link="body"/>
        <child link="t265_pose_frame"/>	
        <origin xyz="-0.485 -0.051 0" rpy="-1.570 0 0"/>
        <axis xyz="1 0 0"/>
    </joint>	   

    <link name="left_tof_link"/>
    <joint name="body_to_left_tof" type="fixed">
        <parent link="body"/>
        <child link="left_tof_link"/>
        <origin xyz="0.485 0.094 0.048" rpy="1.22 0.485 -0.57"/>
        <axis xyz="1 0 0"/>
    </joint>

    <link name="right_tof_link"/>
    <joint name="body_to_right_tof" type="fixed">
        <parent link="body"/>
        <child link="right_tof_link"/>
        <origin xyz="0.485 -0.094 0.048" rpy="1.851 0.485 0.57"/>
        <axis xyz="1 0 0"/>
    </joint>

    <!-- <link name="s_right_tof_link"/>
    <joint name="body_to_s_right_tof" type="fixed">
        <parent link="body"/>
        <child link="s_right_tof_link"/>
        <origin xyz="-0.082 -0.119 0.061" rpy="3.1415 0.524 -1.57"/>
        <axis xyz="1 0 0"/>
    </joint>

    <link name="s_left_tof_link"/>
    <joint name="body_to_s_left_tof" type="fixed">
        <parent link="body"/>
        <child link="s_left_tof_link"/>
        <origin xyz="-0.082 0.119 0.061" rpy="0 0.524 1.57"/>
        <axis xyz="1 0 0"/>
    </joint>

    <link name="rear_tof_link"/>
    <joint name="body_to_rear_tof" type="fixed">
        <parent link="body"/>
        <child link="rear_tof_link"/>
        <origin xyz="-0.5 0 0" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
    </joint> -->

    <!-- Fake front depth camera -->
    <!-- <link name="synth_depth_left_camera_frame" />
    <joint name="synth_depth_left_camera_joint" type="fixed">
        <parent link="front_sensor_head_link"/>
        <child link="synth_depth_left_camera_frame"/>
	    <origin xyz="0 0 -0.15" rpy="0 0.25 0" />
	    <axis xyz="0 1 0"/>
    </joint>

    <link name="synth_depth_left_camera_optical_frame"/>
    <joint name="synth_depth_left_camera_optical_frame" type="fixed">
        <origin xyz="0 0 0" rpy="-1.57079632679 0.0 -1.57079632679"/>
        <parent link="synth_depth_left_camera_frame"/>
        <child link="synth_depth_left_camera_optical_frame"/>
    </joint> -->


   <!-- Frame corners -->
   <!-- Top corners -->
<!--
   <link name="flt_corner_link"/>
    <joint name="body_to_flt_corner_link" type="fixed">
        <parent link="body"/>
        <child link="flt_corner_link"/>
       <origin xyz="0.425  0.115 0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>


    <link name="frt_corner_link"/>
    <joint name="body_to_frt_corner_link" type="fixed">
        <parent link="body"/>
        <child link="frt_corner_link"/>
       <origin xyz="0.425  -0.115 0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>
    
    <link name="brt_corner_link"/>
    <joint name="body_to_brt_corner_link" type="fixed">
        <parent link="body"/>
        <child link="brt_corner_link"/>
       <origin xyz="-0.425  -0.115 0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>


    <link name="blt_corner_link"/>
    <joint name="body_to_blt_corner_link" type="fixed">
        <parent link="body"/>
        <child link="blt_corner_link"/>
       <origin xyz="-0.425  0.115 0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>
-->
    <!-- bottom corners-->
   <!--
    <link name="flb_corner_link"/>
    <joint name="body_to_flb_corner_link" type="fixed">
        <parent link="body"/>
        <child link="flb_corner_link"/>
       <origin xyz="0.425  0.115 -0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>


    <link name="frb_corner_link"/>
    <joint name="body_to_frb_corner_link" type="fixed">
        <parent link="body"/>
        <child link="frb_corner_link"/>
       <origin xyz="0.425  -0.115 -0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>

    <link name="brb_corner_link"/>
    <joint name="body_to_brb_corner_link" type="fixed">
        <parent link="body"/>
        <child link="brb_corner_link"/>
       <origin xyz="-0.425  -0.115 -0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>

    <link name="blb_corner_link"/>
    <joint name="body_to_blb_corner_link" type="fixed">
        <parent link="body"/>
        <child link="blb_corner_link"/>
       <origin xyz="-0.425  0.115 -0.09" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 0 0"/>
    </joint>
-->
</robot>
