87 lines
3.9 KiB
XML
87 lines
3.9 KiB
XML
<?xml version="1.0"?>
|
|
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
<xacro:macro name="depthCamera" params="camID name *origin">
|
|
<joint name="camera_joint_${name}" type="fixed">
|
|
<xacro:insert_block name="origin"/>
|
|
<parent link="trunk"/>
|
|
<child link="camera_${name}"/>
|
|
</joint>
|
|
|
|
<link name="camera_${name}">
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<box size=".001 .001 .001"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="file://$(find go1_description)/meshes/depthCamera.dae" scale="1 1 1"/>
|
|
</geometry>
|
|
</visual>
|
|
|
|
<inertial>
|
|
<mass value="1e-5" />
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
|
</inertial>
|
|
</link>
|
|
|
|
<joint name="camera_optical_joint_${name}" type="fixed">
|
|
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
|
|
<parent link="camera_${name}"/>
|
|
<child link="camera_optical_${name}"/>
|
|
</joint>
|
|
|
|
<link name="camera_optical_${name}">
|
|
</link>
|
|
|
|
<!-- <gazebo reference="camera_${name}">-->
|
|
<!-- <!– <material>Gazebo/Black</material> –>-->
|
|
<!-- <sensor name="camera_${name}_camera" type="depth">-->
|
|
<!-- <update_rate>16</update_rate>-->
|
|
<!-- <camera>-->
|
|
<!-- <horizontal_fov>2.094</horizontal_fov>-->
|
|
<!-- <image>-->
|
|
<!-- <width>928</width>-->
|
|
<!-- <height>800</height>-->
|
|
<!-- <format>R8G8B8</format>-->
|
|
<!-- </image>-->
|
|
<!-- <clip>-->
|
|
<!-- <near>0.1</near>-->
|
|
<!-- <far>5</far>-->
|
|
<!-- </clip>-->
|
|
<!-- </camera>-->
|
|
<!-- <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">-->
|
|
<!-- <baseline>0.025</baseline>-->
|
|
<!-- <alwaysOn>true</alwaysOn>-->
|
|
<!-- <updateRate>0.0</updateRate>-->
|
|
<!-- <cameraName>camera_${name}_ir</cameraName>-->
|
|
<!-- <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>-->
|
|
<!-- <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>-->
|
|
<!-- <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>-->
|
|
<!-- <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>-->
|
|
<!-- <!– <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> –>-->
|
|
<!-- <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>-->
|
|
<!-- <frameName>camera_optical_${name}</frameName>-->
|
|
<!-- <pointCloudCutoff>0.1</pointCloudCutoff>-->
|
|
<!-- <pointCloudCutoffMax>1.5</pointCloudCutoffMax>-->
|
|
<!-- <distortionK1>0.0</distortionK1>-->
|
|
<!-- <distortionK2>0.0</distortionK2>-->
|
|
<!-- <distortionK3>0.0</distortionK3>-->
|
|
<!-- <distortionT1>0.0</distortionT1>-->
|
|
<!-- <distortionT2>0.0</distortionT2>-->
|
|
<!-- <CxPrime>0</CxPrime>-->
|
|
<!-- <Cx>0.0045</Cx>-->
|
|
<!-- <Cy>0.0039</Cy>-->
|
|
<!-- <focalLength>0</focalLength>-->
|
|
<!-- <hackBaseline>0</hackBaseline>-->
|
|
<!-- </plugin>-->
|
|
<!-- </sensor>-->
|
|
<!-- </gazebo>-->
|
|
|
|
</xacro:macro>
|
|
</robot> |