137 lines
5.2 KiB
XML
137 lines
5.2 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
|
<xacro:macro name="intel_realsense_mount" params="prefix frame topic parent_link">
|
|
<material name="white">
|
|
<color rgba="1 1 1 1"/>
|
|
</material>
|
|
<material name="black">
|
|
<color rgba="0 0 0 1"/>
|
|
</material>
|
|
|
|
<xacro:macro name="intel_realsense" params="
|
|
frame:=camera topic:=realsense
|
|
h_fov:=1.5184351666666667 v_fov:=1.0122901111111111
|
|
min_range:=0.105 max_range:=15
|
|
width:=848 height:=480
|
|
update_rate:=15
|
|
robot_namespace:=/">
|
|
|
|
<!-- this link is the origin for the camera's data -->
|
|
<link name="${frame}" />
|
|
|
|
<!--
|
|
The gazebo plugin aligns the depth data with the Z axis, with X=left and Y=up
|
|
ROS expects the depth data along the X axis, with Y=left and Z=up
|
|
This link only exists to give the gazebo plugin the correctly-oriented frame
|
|
-->
|
|
<link name="${frame}_link" />
|
|
<joint name="${frame}_link_joint" type="fixed">
|
|
<parent link="${frame}"/>
|
|
<child link="${frame}_link"/>
|
|
<origin xyz="0.0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
|
|
</joint>
|
|
|
|
|
|
<!-- Some arbitrary link to simulate the realsense -->
|
|
<link name="${prefix}_link" />
|
|
<link name="${prefix}_depth_frame" />
|
|
<link name="${prefix}_depth_optical_frame" />
|
|
|
|
<joint name="${frame}_${prefix}_link_joint" type="fixed">
|
|
<parent link="${frame}"/>
|
|
<child link="${prefix}_link"/>
|
|
<origin xyz="0.0 0 0" rpy="0 0 0"/>
|
|
</joint>
|
|
|
|
<joint name="${prefix}_link_depth_frame_joint" type="fixed">
|
|
<parent link="${prefix}_link"/>
|
|
<child link="${prefix}_depth_frame"/>
|
|
<origin xyz="0.0 0 0" rpy="0 0 0"/>
|
|
</joint>
|
|
|
|
<joint name="${prefix}_depth_frame_depth_optical_frame_joint" type="fixed">
|
|
<parent link="${prefix}_depth_frame"/>
|
|
<child link="${prefix}_depth_optical_frame"/>
|
|
<origin xyz="0.0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
|
|
</joint>
|
|
|
|
|
|
<gazebo reference="${frame}">
|
|
<turnGravityOff>true</turnGravityOff>
|
|
<sensor type="depth" name="${prefix}_realsense_depth">
|
|
<update_rate>${update_rate}</update_rate>
|
|
<camera>
|
|
<!-- 75x65 degree FOV for the depth sensor -->
|
|
<horizontal_fov>${h_fov}</horizontal_fov>
|
|
<vertical_fov>${v_fov}</vertical_fov>
|
|
|
|
<image>
|
|
<width>${width}</width>
|
|
<height>${height}</height>
|
|
<!-- TODO: check what format the Realsense hardware delivers and set this to match! -->
|
|
<format>RGB_INT8</format>
|
|
</image>
|
|
<clip>
|
|
<!-- give the color sensor a maximum range of 50m so that the simulation renders nicely -->
|
|
<near>0.01</near>
|
|
<far>50.0</far>
|
|
</clip>
|
|
</camera>
|
|
<plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
|
|
<baseline>0.2</baseline>
|
|
<alwaysOn>true</alwaysOn>
|
|
<updateRate>${update_rate}</updateRate>
|
|
<cameraName>${topic}</cameraName>
|
|
<imageTopicName>color/image_raw</imageTopicName>
|
|
<cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
|
|
<depthImageTopicName>depth/image_rect_raw</depthImageTopicName>
|
|
<depthImageInfoTopicName>depth/camera_info</depthImageInfoTopicName>
|
|
<pointCloudTopicName>depth/color/points</pointCloudTopicName>
|
|
<frameName>${frame}_link</frameName>
|
|
<pointCloudCutoff>${min_range}</pointCloudCutoff>
|
|
<pointCloudCutoffMax>${max_range}</pointCloudCutoffMax>
|
|
<distortionK1>0.00000001</distortionK1>
|
|
<distortionK2>0.00000001</distortionK2>
|
|
<distortionK3>0.00000001</distortionK3>
|
|
<distortionT1>0.00000001</distortionT1>
|
|
<distortionT2>0.00000001</distortionT2>
|
|
<CxPrime>0</CxPrime>
|
|
<Cx>0</Cx>
|
|
<Cy>0</Cy>
|
|
<focalLength>0</focalLength>
|
|
<hackBaseline>0</hackBaseline>
|
|
</plugin>
|
|
</sensor>
|
|
</gazebo>
|
|
</xacro:macro>
|
|
|
|
<link name="${prefix}_realsense_lens">
|
|
<visual>
|
|
<!--
|
|
the model's origin is in the middle & it's rotated to lie in
|
|
the optical standard with X left, Y up, and Z forward
|
|
-->
|
|
<origin xyz="0.0115 0 0.0100" rpy="1.570796 0 1.570796" />
|
|
<geometry>
|
|
<!-- Origin of this mesh is the base of the bracket. -->
|
|
<box size="0.042 0.042 0.023" />
|
|
</geometry>
|
|
<material name="black" />
|
|
</visual>
|
|
</link>
|
|
|
|
<joint type="fixed" name="${prefix}_realsense_lens_joint">
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<parent link="${parent_link}" />
|
|
<child link="${prefix}_realsense_lens" />
|
|
</joint>
|
|
<joint type="fixed" name="${prefix}_realsense_joint">
|
|
<origin xyz="0.025 0 0" rpy="0 0 0" />
|
|
<parent link="${prefix}_realsense_lens" />
|
|
<child link="${prefix}_realsense" />
|
|
</joint>
|
|
|
|
<xacro:intel_realsense frame="${prefix}_realsense" topic="${topic}"/>
|
|
</xacro:macro>
|
|
</robot>
|