From f25e8892ecf52d74ce1cbb49740eb8996bfc1971 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 17 Jul 2023 04:50:35 -0300
Subject: [PATCH] Port d435 depth camera to Humble.

---
 tatuira_description/launch/gazebo.launch.xml       |  4 +--
 tatuira_description/package.xml                    |  1 +
 .../urdf/{d435.xacro => d435.urdf.xacro}           | 30 ++++++++++++----------
 ...ebo_control.xacro => gazebo_control.urdf.xacro} |  0
 ...{ground_truth.xacro => ground_truth.urdf.xacro} |  0
 ...{ros2_control.xacro => ros2_control.urdf.xacro} |  0
 tatuira_description/urdf/tatuira.urdf.xacro        |  8 +++---
 7 files changed, 24 insertions(+), 19 deletions(-)
 rename tatuira_description/urdf/{d435.xacro => d435.urdf.xacro} (75%)
 rename tatuira_description/urdf/{gazebo_control.xacro => gazebo_control.urdf.xacro} (100%)
 rename tatuira_description/urdf/{ground_truth.xacro => ground_truth.urdf.xacro} (100%)
 rename tatuira_description/urdf/{ros2_control.xacro => ros2_control.urdf.xacro} (100%)

diff --git a/tatuira_description/launch/gazebo.launch.xml b/tatuira_description/launch/gazebo.launch.xml
index d8aa4cc..e4293e0 100644
--- a/tatuira_description/launch/gazebo.launch.xml
+++ b/tatuira_description/launch/gazebo.launch.xml
@@ -35,9 +35,9 @@
 
 	<include file="$(find-pkg-share tatuira_description)/launch/tatuira.launch.xml">
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-		<arg name="d435" value="$(var d435)" />
+		<arg name="d435" value="$(var d435)"/>
 	</include>
 
 	<node name="tatuira_spawner" pkg="gazebo_ros" exec="spawn_entity.py"
-		args="-topic robot_description -entity tatuira" />
+		args="-topic robot_description -entity tatuira"/>
 </launch>
diff --git a/tatuira_description/package.xml b/tatuira_description/package.xml
index 7253dc7..748f52c 100644
--- a/tatuira_description/package.xml
+++ b/tatuira_description/package.xml
@@ -23,5 +23,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/tatuira_description/urdf/d435.xacro b/tatuira_description/urdf/d435.urdf.xacro
similarity index 75%
rename from tatuira_description/urdf/d435.xacro
rename to tatuira_description/urdf/d435.urdf.xacro
index eda5179..b98b7a1 100644
--- a/tatuira_description/urdf/d435.xacro
+++ b/tatuira_description/urdf/d435.urdf.xacro
@@ -7,34 +7,36 @@
 	<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="awac_link">
-			<origin xyz="0 0 0.175" rpy="0.0 0.0 ${pi/2}" />
+
+		<xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
+
+		<xacro:sensor_d435 parent="awac_link" use_nominal_extrinsics="false" add_plug="false" use_mesh="true">
+			<origin xyz="0 0 0.175" rpy="0.0 0.0 ${pi/2}"/>
 		</xacro:sensor_d435>
 
-		<link name="plugin_camera_link" />
+		<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}" />
+			<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>trun</always_on>
-				<update_rate>15</update_rate> <!--or 6 or 30 -->
+				<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 -->
+				<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-->
+					<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 -->
+						<width>640</width>	<!--or 1280 -->
+						<height>480</height>	<!--or 720 -->
 						<format>R8G8B8</format>
 						<!--format>R_FLOAT32</format-->
 					</image>
@@ -52,7 +54,7 @@
 				<!-- Gazebo Classic -->
 				<xacro:if value="${hardware == 'gazebo'}">
 					<plugin name="camera_plugin" filename="libgazebo_ros_camera.so">
-						<baseline>0.05</baseline>  <!-- dist between sensors -->
+						<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>
@@ -74,6 +76,8 @@
 							<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>
diff --git a/tatuira_description/urdf/gazebo_control.xacro b/tatuira_description/urdf/gazebo_control.urdf.xacro
similarity index 100%
rename from tatuira_description/urdf/gazebo_control.xacro
rename to tatuira_description/urdf/gazebo_control.urdf.xacro
diff --git a/tatuira_description/urdf/ground_truth.xacro b/tatuira_description/urdf/ground_truth.urdf.xacro
similarity index 100%
rename from tatuira_description/urdf/ground_truth.xacro
rename to tatuira_description/urdf/ground_truth.urdf.xacro
diff --git a/tatuira_description/urdf/ros2_control.xacro b/tatuira_description/urdf/ros2_control.urdf.xacro
similarity index 100%
rename from tatuira_description/urdf/ros2_control.xacro
rename to tatuira_description/urdf/ros2_control.urdf.xacro
diff --git a/tatuira_description/urdf/tatuira.urdf.xacro b/tatuira_description/urdf/tatuira.urdf.xacro
index 602e718..a690694 100644
--- a/tatuira_description/urdf/tatuira.urdf.xacro
+++ b/tatuira_description/urdf/tatuira.urdf.xacro
@@ -73,10 +73,10 @@
 
 	<xacro:back_macro name="left" parent="center_0" reflect="-1" />
 
-	<xacro:include filename="ros2_control.xacro"/>
-	<xacro:include filename="gazebo_control.xacro"/>
+	<xacro:include filename="ros2_control.urdf.xacro"/>
+	<xacro:include filename="gazebo_control.urdf.xacro"/>
 
-	<xacro:include filename="ground_truth.xacro"/>
+	<xacro:include filename="ground_truth.urdf.xacro"/>
 
-	<xacro:include filename="d435.xacro"/>
+	<xacro:include filename="d435.urdf.xacro"/>
 </robot>
-- 
2.12.0