123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124 |
- <?xml version="1.0" encoding="UTF-8"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
- <xacro:include filename="$(find hector_xacro_tools)/urdf/sensor_macros.urdf.xacro" />
-
- <xacro:macro name="seek_thermal_compact_camera_model" params="name parent *origin add_geometry">
-
- <joint name="${name}_joint" type="fixed">
- <xacro:insert_block name="origin" />
- <parent link="${parent}" />
- <child link="${name}_frame" />
- </joint>
-
- <link name="${name}_frame">
- <xacro:inertial_sphere mass="0.03" diameter="0.02" />
-
- <xacro:if value="${add_geometry}">
- <visual>
- <origin xyz="0 0.0 -0.025" rpy="0 0 0" />
- <geometry>
- <box size="0.02 0.03 0.0375" />
- </geometry>
- <material name="DarkGrey">
- <color rgba="0.3 0.3 0.3 1" />
- </material>
- </visual>
- <visual>
- <origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
- <geometry>
- <cylinder length="0.02" radius="0.017" />
- </geometry>
- <material name="DarkGrey" />
- </visual>
- <visual>
- <origin xyz="0.01 0 0" rpy="0 ${M_PI/2} 0" />
- <geometry>
- <cylinder length="0.018" radius="0.011" />
- </geometry>
- <material name="DarkGrey" />
- </visual>
-
- <collision>
- <origin xyz="0.0055 0 -0.013375" rpy="0 0 0" />
- <geometry>
- <box size="0.031 0.034 0.06075" />
- </geometry>
- </collision>
-
- <xacro:if value="$(arg add_raycast_self_filter_geom)">
- <collision>
- <origin xyz="0.0055 0 -0.013375" rpy="0 0 0" />
- <geometry>
- <cylinder length="0.06075" radius="0.023"/>
- </geometry>
- </collision>
- </xacro:if>
- </xacro:if>
- </link>
-
- <xacro:add_optical_frame name="${name}" />
-
- <gazebo reference="${name}_frame">
- <material>Gazebo/Grey</material>
- </gazebo>
-
- </xacro:macro>
-
- <xacro:macro name="seek_thermal_compact_gazebo_plugin_macro" params="name parent *origin">
- <gazebo reference="${name}_frame">
- <sensor type="camera" name="thermal_camera_sensor">
- <update_rate>10</update_rate>
- <camera>
- <horizontal_fov>${90.0*M_PI/180.0}</horizontal_fov>
- <image>
- <format>R8G8B8</format>
- <width>206</width>
- <height>156</height>
- </image>
- <clip>
- <near>0.01</near>
- <far>10</far>
- </clip>
- </camera>
- <plugin name="thermal_camera_controller" filename="libgazebo_ros_thermal_camera.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>10</updateRate>
- <cameraName>${name}</cameraName>
- <imageTopicName>image_raw</imageTopicName>
- <cameraInfoTopicName>camera_info</cameraInfoTopicName>
- <frameName>${name}_optical_frame</frameName>
- </plugin>
- </sensor>
- </gazebo>
-
- </xacro:macro>
-
- <xacro:macro name="seek_thermal_compact_camera" params="name parent *origin">
-
- <xacro:seek_thermal_compact_camera_model name="${name}" parent="${parent}" add_geometry="true">
- <xacro:insert_block name="origin" />
- </xacro:seek_thermal_compact_camera_model>
-
- <xacro:seek_thermal_compact_gazebo_plugin_macro name="${name}" parent="${parent}">
- <xacro:insert_block name="origin" />
- </xacro:seek_thermal_compact_gazebo_plugin_macro>
-
- </xacro:macro>
-
- <xacro:macro name="seek_thermal_compact_camera_no_geom" params="name parent *origin">
-
- <xacro:seek_thermal_compact_camera_model name="${name}" parent="${parent}" add_geometry="false">
- <xacro:insert_block name="origin" />
- </xacro:seek_thermal_compact_camera_model>
-
- <xacro:seek_thermal_compact_gazebo_plugin_macro name="${name}" parent="${parent}">
- <xacro:insert_block name="origin" />
- </xacro:seek_thermal_compact_gazebo_plugin_macro>
-
- </xacro:macro>
- </robot>
|