123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:property name="M_PI" value="3.1415926535897931" />
-
- <!--
- The asus_camera_model macro only adds the model, it does not also add
- the openni gazebo plugin. See the 'asus_camera' macro below for that
- -->
- <xacro:macro name="asus_camera_model" params="name parent *origin">
- <joint name="${name}_joint" type="fixed">
- <xacro:insert_block name="origin" />
- <parent link="${parent}"/>
- <child link="${name}_link"/>
- </joint>
-
- <link name="${name}_link">
- <inertial>
- <mass value="0.200" />
- <origin xyz="0 0 0" rpy="0 0 0" />
- <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://hector_sensors_description/meshes/asus_camera/asus_camera_simple.dae"/>
- </geometry>
- </visual>
- <!--
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <box size="0.035 0.185 0.025"/>
- </geometry>
- </collision>
- -->
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://hector_sensors_description/meshes/asus_camera/asus_camera_simple.dae"/>
- </geometry>
- </collision>
- </link>
- <joint name="${name}_depth_joint" type="fixed">
- <origin xyz="0.0 0.049 0.0" rpy="0 0 0" />
- <parent link="${name}_link" />
- <child link="${name}_depth_frame"/>
- </joint>
- <link name="${name}_depth_frame"/>
- <joint name="${name}_depth_optical_joint" type="fixed">
- <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
- <parent link="${name}_depth_frame" />
- <child link="${name}_depth_optical_frame"/>
- </joint>
- <link name="${name}_depth_optical_frame"/>
- <joint name="${name}_rgb_joint" type="fixed">
- <origin xyz="0.0 0.022 0.0" rpy="0 0 0" />
- <parent link="${name}_link" />
- <child link="${name}_rgb_frame"/>
- </joint>
- <link name="${name}_rgb_frame"/>
- <joint name="${name}_rgb_optical_joint" type="fixed">
- <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
- <parent link="${name}_rgb_frame" />
- <child link="${name}_rgb_optical_frame"/>
- </joint>
- <link name="${name}_rgb_optical_frame"/>
- </xacro:macro>
- <!--
- The asus_camera macro only adds the model, and also adds
- the openni gazebo plugin.
- -->
- <xacro:macro name="asus_camera" params="name parent *origin">
- <xacro:asus_camera_model name="${name}" parent="${parent}">
- <xacro:insert_block name="origin" />
- </xacro:asus_camera_model>
-
- <!-- ASUS Xtion PRO camera for simulation -->
- <gazebo reference="${name}_depth_frame">
- <sensor type="depth" name="${name}">
- <update_rate>20</update_rate>
- <camera>
- <horizontal_fov>${62.8 * M_PI/180.0}</horizontal_fov>
- <image>
- <format>R8G8B8</format>
- <width>640</width>
- <height>480</height>
- </image>
- <clip>
- <near>0.5</near>
- <far>9</far>
- </clip>
- </camera>
- <plugin name="${name}_camera_controller" filename="libgazebo_ros_openni_kinect.so">
- <imageTopicName>${name}/rgb/image_raw</imageTopicName>
- <cameraInfoTopicName>${name}/rgb/camera_info</cameraInfoTopicName>
- <depthImageTopicName>${name}/depth/image_raw</depthImageTopicName>
- <depthImageCameraInfoTopicName>${name}/depth/camera_info</depthImageCameraInfoTopicName>
- <pointCloudTopicName>${name}/depth/points</pointCloudTopicName>
- <frameName>${name}_depth_optical_frame</frameName>
- </plugin>
- </sensor>
- </gazebo>
- </xacro:macro>
- </robot>
-
|