From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 17 Jul 2023 07:49:26 +0000 (-0300)
Subject: Port d435 depth camera to Humble.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b5a4c5935f38c3665fa10e24c7c3e1daa3518ba1;p=twil.git

Port d435 depth camera to Humble.
---

diff --git a/twil_description/package.xml b/twil_description/package.xml
index b968f79..78af7c1 100644
--- a/twil_description/package.xml
+++ b/twil_description/package.xml
@@ -18,5 +18,6 @@
   <export>
     <build_type>ament_cmake</build_type>
     <gazebo_ros gazebo_model_path="${prefix}/.."/>
+    <gazebo_ros gazebo_model_path="/opt/ros/humble/share/realsense2_description//.."/>
   </export>
 </package>
diff --git a/twil_description/xacro/d435.urdf.xacro b/twil_description/xacro/d435.urdf.xacro
new file mode 100644
index 0000000..5f66114
--- /dev/null
+++ b/twil_description/xacro/d435.urdf.xacro
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<xacro:arg name="hardware" default="gazebo"/>
+	<xacro:property name="hardware" value="$(arg hardware)"/>
+
+	<xacro:arg name="d435" default="false"/>
+
+	<xacro:if value="$(arg d435)">
+
+		<xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
+
+		<xacro:sensor_d435 parent="tower" use_nominal_extrinsics="false" add_plug="false" use_mesh="true">
+			<origin xyz="0 0 0.876" rpy="0.0 0.0 0.0"/>
+		</xacro:sensor_d435>
+
+		<link name="plugin_camera_link"/>
+
+		<joint name="plugin_camera_joint" type="fixed">
+			<parent link="camera_link"/>
+			<child link="plugin_camera_link"/>
+			<origin xyz="0.0 0.0 0.0" rpy="${-pi/2} 0.0 ${-pi/2}"/>
+    		</joint>
+
+		<gazebo reference="camera_link">
+			<sensor name="camera" type="depth">
+			<!--sensor name="camera" type="depth_camera"-->
+				<always_on>true</always_on>
+				<update_rate>15</update_rate>	<!--or 6 or 30 -->
+				<visualize>false</visualize>
+				<triggered>false</triggered>
+				<trigger_topic>trigger</trigger_topic>
+				<topic>depth_camera</topic>	<!-- This is only used by Gazebo Ignition -->
+				<ignition_frame_id>camera_link</ignition_frame_id>
+				<camera>
+					<horizontal_fov>1.309</horizontal_fov>	<!--75 deg at 480p  or 87 deg at 720p-->
+					<image>
+						<width>640</width>	<!--or 1280 -->
+						<height>480</height>	<!--or 720 -->
+						<format>R8G8B8</format>
+						<!--format>R_FLOAT32</format-->
+					</image>
+					<clip>
+						<near>0.05</near>
+						<far>10</far>
+					</clip>
+					<noise>
+						<type>gaussian</type>
+						<mean>0.0</mean>
+						<stddev>0.02</stddev>
+					</noise>
+				</camera>
+
+				<!-- Gazebo Classic -->
+				<xacro:if value="${hardware == 'gazebo'}">
+					<plugin name="camera_plugin" filename="libgazebo_ros_camera.so">
+						<baseline>0.05</baseline>	<!-- dist between sensors -->
+						<!-- Keep this zero, update_rate in the parent <sensor> tag 
+						will control the frame rate. -->
+						<update_rate>0.0</update_rate>
+						<camera_name>camera</camera_name>
+						<frame_name>camera_link</frame_name>
+						<hack_baseline>0.05</hack_baseline>
+						<!--P_cx>0.0</P_cx?
+						<P_cy>0.0</P_cy>
+						<P_fy>0.0</P_fy>
+						<Tx>0.0</Tx>
+						<Ty>0.0</Ty>
+						<rectification_matrix>1 0 0 0 1 0 0 0 1</rectification_matrix-->
+						<ros>
+							<!--namespace>namespace</namespace-->
+							<remapping>/camera/image_raw:=/camera/color/image_raw</remapping>
+							<remapping>/camera/camera_info:=camera/color/camera_info</remapping>
+							<remapping>/camera/depth/image_raw:=/camera/depth/image_raw</remapping>
+							<remapping>/camera_depth/camera_info:=/camera/depth/camera_info</remapping>
+							<remapping>/camera/points:=camera/depth/points</remapping>
+						</ros>
+						<!--pointCloudCutoff>0.175</pointCloudCutoff--> <!-- or 0.28 at 720p -->
+						<min_depth>0.05</min_depth>
+						<max_depth>10</max_depth>
+					</plugin>
+				</xacro:if>
+			</sensor>
+		</gazebo>
+	</xacro:if>
+</robot>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 5af782c..8107882 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -178,68 +178,7 @@
   </xacro:if>
 
   <xacro:if value="$(arg d435)">
-    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
-    <xacro:sensor_d435 parent="tower">
-      <origin xyz="0 0 0.876" rpy="0.0 0.0 0.0" />
-    </xacro:sensor_d435>
-
-    <link name="plugin_camera_link" />
-
-    <joint name="plugin_camera_joint" type="fixed">
-      <parent link="camera_link"/>
-      <child link="plugin_camera_link" />
-      <origin xyz="0.0 0.0 0.0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
-    </joint>
-
-    <gazebo reference="camera_link">
-      <sensor name="camera" type="depth">
-        <update_rate>15</update_rate> <!--or 6 or 30 -->
-        <camera>
-          <horizontal_fov>1.309</horizontal_fov> <!--75 deg at 480p  or 87 deg at 720p-->
-          <image>
-            <width>640</width> <!--or 1280 -->
-            <height>480</height> <!--or 720 -->
-            <format>R8G8B8</format>
-          </image>
-          <clip>
-            <near>0.05</near>
-            <far>10</far>
-          </clip>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.02</stddev>
-          </noise>
-        </camera>
-
-        <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
-          <baseline>0.05</baseline>  <!-- dist between sensors -->
-          <alwaysOn>true</alwaysOn>
-          <!-- Keep this zero, update_rate in the parent <sensor> tag
-            will control the frame rate. -->
-          <updateRate>0.0</updateRate>
-          <cameraName>camera</cameraName>
-          <imageTopicName>/camera/color/image_raw</imageTopicName>
-          <cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
-          <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
-          <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
-          <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
-          <frameName>plugin_camera_link</frameName>
-          <pointCloudCutoff>0.175</pointCloudCutoff> <!-- or 0.28 at 720p -->
-	  <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
-          <distortionK1>0</distortionK1>
-          <distortionK2>0</distortionK2>
-          <distortionK3>0</distortionK3>
-          <distortionT1>0</distortionT1>
-          <distortionT2>0</distortionT2>
-          <CxPrime>0</CxPrime>
-          <Cx>0</Cx>
-          <Cy>0</Cy>
-          <focalLength>0</focalLength>
-          <hackBaseline>0</hackBaseline>
-        </plugin>
-      </sensor>
-    </gazebo>
+	<xacro:include filename="$(find twil_description)/xacro/d435.urdf.xacro"/>
   </xacro:if>
 
 </robot>