<?xml version="1.0"?>
<!--

  Copyright (c) 2021 PAL Robotics S.L. All rights reserved.
  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at
      http://www.apache.org/licenses/LICENSE-2.0
  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<robot  xmlns:xacro="http://ros.org/wiki/xacro"
        name="tiago" >
    <xacro:arg name="arm" default="true"/> <!-- true, false -->
    <xacro:arg name="wrist_model" default="wrist-2010"/>
    <xacro:arg name="end_effector" default="pal-hey5"/> <!-- false, pal-gripper, pal-hey5, schunk-wsg-->
    <xacro:arg name="ft_sensor" default="schunk-ft"/> <!-- false, schunk-ft -->
    <xacro:arg name="laser_model" default="sick-571"/> <!-- false, hokuyo, sick-551, sick-561, sick-571-->
    <xacro:arg name="camera_model" default="orbbec-astra"/> <!-- false, orbbec-astra, orbbec-astra-pro, asus-xtion -->
    <xacro:arg name="has_thermal_camera" default="false"/>
    <xacro:arg name="multiple" default="false"/>
    <xacro:arg name="description_calibration_dir" default="$(find tiago_description_calibration)/urdf/calibration"/>
    <xacro:arg name="extrinsic_calibration_dir" default="$(find tiago_description_calibration)/urdf/calibration"/>
    <xacro:arg name="namespace" default=""/>
    <xacro:arg name="no_safety_eps" default="false"/>
    <xacro:arg name="base_type" default="pmb2"/> <!-- pmb2, omni_base -->
    <xacro:property name="wrist_model" value="$(arg wrist_model)" />
    <xacro:property name="end_effector" value="$(arg end_effector)" />
    <xacro:property name="ft_sensor" value="$(arg ft_sensor)" />
    <xacro:property name="laser_model" value="$(arg laser_model)" />
    <xacro:property name="camera_model" value="$(arg camera_model)" />
    <xacro:property name="is_multiple" value="$(arg multiple)" />
    <xacro:property name="nsp" value="$(arg namespace)" />
    <xacro:property name="base_type" value="$(arg base_type)" />
    <xacro:property name="no_safety_eps" value="$(arg no_safety_eps)" />

  <xacro:if value="$(arg arm)">
    <xacro:if value="${wrist_model not in ['wrist-2010', 'wrist-2017']}">
      <xacro:wrong_wrist_model/>
    </xacro:if>
  </xacro:if>
  <xacro:if value="${end_effector not in ['false', False, 'pal-gripper', 'pal-hey5', 'schunk-wsg', 'robotiq-2f-85', 'robotiq-2f-140', 'robotiq-epick', 'custom']}">
    <xacro:wrong_end_effector/>
  </xacro:if>
  <xacro:if value="${ft_sensor not in ['false', False, 'schunk-ft']}">
    <xacro:wrong_ft_sensor/>
  </xacro:if>
  <xacro:if value="${laser_model not in ['false', False, 'sick-551', 'sick-561', 'sick-571', 'hokuyo']}">
    <xacro:wrong_laser_model/>
  </xacro:if>
  <xacro:if value="${camera_model not in ['false', False, 'orbbec-astra', 'orbbec-astra-pro', 'asus-xtion']}">
    <xacro:wrong_camera_model/>
  </xacro:if>
  <xacro:if value="${base_type not in ['pmb2', 'omni_base']}">
    <xacro:wrong_base_type/>
  </xacro:if>
        <!-- The properties below are modified using empy http://www.alcyone.com/software/empy/
             from the create_robot.py tool. Handle them with care -->
   <xacro:property name="has_arm"                 value="$(arg arm)"/>
   <xacro:property name="has_schunk_ft"           value="${ft_sensor == 'schunk-ft'}"/>
   <xacro:property name="has_ft_sensor"           value="${has_schunk_ft}"/> <!-- This should be OR'ed with other FT sensors-->
   <xacro:property name="end_effector_link"       value="${'wrist_ft_tool_link' if has_ft_sensor else 'arm_tool_link'}"/>
   <xacro:property name="end_effector_name"       value="${'hand' if end_effector == 'pal-hey5' else 'gripper'}"/>
   <xacro:property name="head_link_name"          value="head_2_link"/>
   <xacro:property name="description_calibration_dir"       value="${arg description_calibration_dir}"/>

  <!-- The following included files set up definitions of parts of the robot body -->
  <!-- Base -->
  <xacro:include filename="$(find ${base_type}_description)/urdf/base/base_sensors.urdf.xacro" />

  <!-- Torso -->
  <xacro:include filename="$(find tiago_description)/urdf/torso/torso.urdf.xacro" />

  <xacro:if value="${has_arm}">
    <!-- Arm -->
    <xacro:include filename="$(find tiago_description)/urdf/arm/arm.urdf.xacro" />
    <xacro:include filename="$(find tiago_description)/urdf/end_effector/end_effector.urdf.xacro" />
  <xacro:include filename="$(arg description_calibration_dir)/calibration_constants.urdf.xacro" />
  </xacro:if>

  <!-- Force Torque sensor -->
  <xacro:if value="${has_ft_sensor}">
    <xacro:include filename="$(find tiago_description)/urdf/sensors/ftsensor.urdf.xacro" />
  </xacro:if>

  <!-- Head -->
  <xacro:include filename="$(find tiago_description)/urdf/head/head.urdf.xacro" />

  <!-- Materials for visualization -->
  <xacro:include filename="$(find tiago_description)/urdf/materials.urdf.xacro" />

  <!-- Generic simulator_gazebo plugins -->
  <xacro:include filename="$(find tiago_description)/gazebo/gazebo.urdf.xacro" />

  <!-- Now we can start using the macros included above to define the actual robot -->
  <xacro:if value="${base_type == 'pmb2'}">
    <xacro:base_sensors name="base" laser_model="$(arg laser_model)" sonars="true" microphone="true"/>
  </xacro:if>
  <xacro:if value="${base_type == 'omni_base'}">
    <xacro:base_sensors name="base" front_laser_model="$(arg laser_model)" rear_laser_model="$(arg laser_model)"/>
  </xacro:if>
  <xacro:tiago_torso name="torso" parent="base_link"/>
  <xacro:head name="head" parent="torso_lift_link" camera_model="$(arg camera_model)" 
	  description_calibration_dir="$(arg description_calibration_dir)" 
	  extrinsic_calibration_dir="$(arg extrinsic_calibration_dir)"
    no_safety_eps="${no_safety_eps}"/>
  <xacro:if value="${has_arm}">
	  <xacro:tiago_arm name="arm" parent="torso_lift_link" wrist_model="${wrist_model}" has_ft_sensor="${has_ft_sensor}" reflect="1" tiago_dual="-1" arm_1_offset="${arm_1_joint_offset}" arm_2_offset="${arm_2_joint_offset}" arm_3_offset="${arm_3_joint_offset}" arm_4_offset="${arm_4_joint_offset}" arm_5_offset="${arm_5_joint_offset}" arm_6_offset="${arm_6_joint_offset}" arm_7_offset="${arm_7_joint_offset}" no_safety_eps="${no_safety_eps}">
        <origin xyz="0.15505 0.014 -0.151" rpy="0 0 ${-90 * deg_to_rad}" />
      </xacro:tiago_arm>
      <xacro:tiago_end_effector name="${end_effector_name}" parent="${end_effector_link}" type="${end_effector}" reflect="1"/>
  </xacro:if>

  <!-- Force Torque sensor -->
  <xacro:if value="${has_ft_sensor}">
      <xacro:ft_sensor name="wrist" parent="arm_tool_link" />
  </xacro:if>

  <!-- Thermal camera -->
  <xacro:if value="$(arg has_thermal_camera)">
    <joint name="thermal_camera_joint" type="fixed">
        <parent link="${head_link_name}"/>
        <child link="thermal_camera"/>
        <origin xyz="0.05 0.15 0" rpy="${0*deg_to_rad} ${-90*deg_to_rad} ${-180*deg_to_rad}" />
    </joint>
    <link name="thermal_camera">
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.0001" />
            <inertia ixx="0.0001"  ixy="0"  ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
    </link>
  </xacro:if>

  <!-- RGBD Laser Link -->
  <joint name="rgbd_laser_joint" type="fixed">
      <parent link="base_footprint"/>
      <child link="rgbd_laser_link"/>
      <origin xyz="-0.9 0 0" rpy="0 0 0" />
  </joint>
  <link name="rgbd_laser_link">
      <inertial>
          <origin xyz="0 0 0" rpy="0 0 0"/>
          <mass value="0.0001" />
          <inertia ixx="0.0001"  ixy="0"  ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
      </inertial>
  </link>

</robot>
