<?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="sensorhead">
        <xacro:macro name="sensorhead">
		<!-- Traking frame comes from the realsesne _pose_frame which will share the link with realsense_link
	    (parent) t265_pose_frame -> frontsensorhead -> extrinsics_offset_link -> body (childeren) -->

    <!-- branch 1 -->
    <!--connect to reset of the body -->
    <link name="t265_rot_link"/>
    <joint name="t265_pose_frame_to_t265_rot" type="fixed">
      <parent link="t265_pose_frame"/>
      <child link="t265_rot_link"/>
      <origin xyz="0 0 0" rpy="-1.57 0 0"/> <!-- Need a Rolled 90--> 
      <axis xyz="1 0 0"/>
    </joint>

    <!-- Define front sensor head. This connects back to the body-->
    <link name="front_sensor_head_link"/>
    <joint name="t265_to_sensor_head" type="fixed">
      <parent link="t265_rot_link"/>
      <child link="front_sensor_head_link"/>
      <origin xyz="-0.06 0.0 -0.0507" rpy="0 0 0"/> <!--  xyz -0.0292 0.0 -0.0507 this is not correct-->
      <axis xyz="1 0 0"/>
    </joint>

    <!-- branch 2 -->    
    <!-- start here: this defines the entry point for t265 based state estimate-->
    <link name="t265_fisheye1_frame"/>
    <link name="t265_pose_frame"/>
    <joint name="t265_fisheye1_frame_to_center" type="fixed">
     <parent link="t265_pose_frame"/>
     <child link="t265_fisheye1_frame"/>
     <origin xyz="0.0 0.032 0.0" rpy="0 0 0"/>
     <axis xyz="1 0 0"/>
    </joint>

     <!-- begin branch 2 from t265_fisheye1_frame -->
     <!-- connect to the rest of the sensors -->	
	<link name="t265_fisheye1_optical_frame"/>
	<joint name="t265_left_frame_to_t265_left_optical" type="fixed">
	   <parent link="t265_fisheye1_frame"/>
       <child link="t265_fisheye1_optical_frame"/>
	   <origin xyz="0.0 0.0 0.0"  rpy="-1.571 0 -1.571"/>
       <axis xyz="1 0 0 "/>
	</joint>
    </xacro:macro>
</robot>
