Thesis-Hector-VR/Hector_v2/Assets/Telemax/hector_components_description/urdf/xm430_sensor_head.urdf.xacro.xml

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hector_tracker_sensor_head">
    
  <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 hector_sensors_description)/urdf/realsense_d435_camera.urdf.xacro" />
  <xacro:include filename="$(find hector_sensors_description)/urdf/seek_thermal_compact.urdf.xacro" />
  
  <xacro:macro name="xm430_sensor_head_macro" params="parent *origin name">
    <xacro:arg name="add_raycast_self_filter_geom" default="false" />
    
    <joint name="${name}_mount_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}" />
      <child link="${name}_mount_link" />
    </joint>
    
    <link name="${name}_mount_link">
      <xacro:inertial_sphere mass="0.09" diameter="0.05" />

      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_0.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="DarkGrey" />
      </visual>
      
      <collision>
        <origin xyz="0.0 -0.01 0.017" rpy="0 0 0" />
        <geometry>
          <box size="0.0285 0.07 0.034" />
        </geometry>
      </collision>
      
      <xacro:if value="$(arg add_raycast_self_filter_geom)">
        <collision>
          <origin xyz="0.0 -0.01 0.017" rpy="${M_PI*0.5} 0 0" />
          <geometry>
            <cylinder length="0.07" radius="0.022"/>
          </geometry>
        </collision>
      </xacro:if>
    </link>
    
    <joint name="${name}_yaw_joint" type="revolute">
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <parent link="${name}_mount_link" />
      <child link="${name}_yaw_link" />
      <axis xyz="0 0 1" />
      <limit lower="${-M_PI/2}" upper="${M_PI/2}" effort="4" velocity="4.81710873" />
    </joint>
    
    <link name="${name}_yaw_link">
      <xacro:inertial_sphere mass="0.09" diameter="0.07" />
      
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0.036"/>
        <geometry>
          <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="DarkGrey" />
      </visual>
      
      <collision>
        <origin xyz="0.0 0.0 0.0625" rpy="0 0 0" />
        <geometry>
          <box size="0.0285 0.034 0.055" />
        </geometry>
      </collision>
      
      <xacro:if value="$(arg add_raycast_self_filter_geom)">
        <collision>
          <origin xyz="0.0 0.0 0.0625" rpy="0 0 0" />
          <geometry>
            <cylinder length="0.055" radius="0.022"/>
          </geometry>
        </collision>
      </xacro:if>
    </link>
    
    <joint name="${name}_pitch_joint" type="revolute">
      <origin xyz="0.0 0.0 0.0765" rpy="${-M_PI/2} 0 0" />
      <parent link="${name}_yaw_link" />
      <child link="${name}_pitch_link" />
      <axis xyz="0 0 1" />
      <limit lower="${-M_PI/2}" upper="${M_PI/2}" effort="4" velocity="4.81710873" />
    </joint>
    
    <link name="${name}_pitch_link">
      <xacro:inertial_sphere mass="0.03" diameter="0.07" />

      <visual>
        <origin rpy="${M_PI/2} ${M_PI} 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_2.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="DarkGrey" />
      </visual>
      
      <collision>
        <origin xyz="0.0 -0.0255 0.0" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.005 0.045" />
        </geometry>
        <material name="DarkGrey" />
      </collision>
      <collision>
        <origin xyz="0.0 -0.0075 0.02" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.035 0.006" />
        </geometry>
        <material name="DarkGrey" />
      </collision>
      <collision>
        <origin xyz="0.0 -0.0075 -0.02" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.035 0.006" />
        </geometry>
        <material name="DarkGrey" />
      </collision>     
      <collision>
        <origin xyz="0.0 -0.0075 0.0" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.035 0.045" />
        </geometry>
      </collision>
      
      <xacro:if value="$(arg add_raycast_self_filter_geom)">
        <collision>
          <origin xyz="0.0 -0.0075 0.0" rpy="0 0 0" />
          <geometry>
            <cylinder length="0.045" radius="0.022"/>
          </geometry>
        </collision>
      </xacro:if>
    </link>

    <gazebo reference="${name}_yaw_link">
      <material>Gazebo/DarkGrey</material>
    </gazebo>
    
    <gazebo reference="${name}_pitch_link">
      <material>Gazebo/DarkGrey</material>
    </gazebo>
    
    <gazebo reference="${name}_mount_link">
      <material>Gazebo/DarkGrey</material>
    </gazebo>
    
    <xacro:joint_standard_transmission name="${name}_yaw_joint" />
    <xacro:joint_standard_transmission name="${name}_pitch_joint" />
    
    <gazebo>
      <plugin name="gazebo_ros_control_select_joints" filename="libgazebo_ros_control_select_joints.so">
        <robotNamespace>${name}_control</robotNamespace>
        <joints>${name}_yaw_joint ${name}_pitch_joint</joints>
      </plugin>
    </gazebo>
    
    <xacro:seek_thermal_compact_camera_no_geom name="${name}_thermal_cam" parent="${name}_pitch_link">
      <origin xyz="-0.01 -0.0692 0" rpy="${0.5*M_PI} 0 0" />
    </xacro:seek_thermal_compact_camera_no_geom>
    
    <xacro:realsense_d435_camera_comprehensive_params name="${name}_rgbd_cam" parent="${name}_pitch_link" image_topic="image_rect_color" depth_points_topic="depth/points" min_range="0.3" max_range="9.0" update_rate="20">
      <origin xyz="0.0253 -0.043 0.0175" rpy="${M_PI*0.5} 0.0 0.0" />
    </xacro:realsense_d435_camera_comprehensive_params>
    
  </xacro:macro>
</robot>