Port d435 depth camera to Humble.

diff --git a/twil_description/package.xml b/twil_description/package.xml
index b968f79..78af7c1 100644 (file)
--- 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 (file)
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 (file)
--- 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>