Enhanced URDF Model

.gitignore

@@ -56,3 +56,6 @@ modules.order
 Module.symvers
 Mkfile.old
 dkms.conf
+
+#Blender Autosave
+*.blend?

src/k4a_ros_device.cpp

@@ -692,6 +692,10 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag
 
   pcd_modifier.resize(point_count);
 
+  // Restore actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1
+  point_cloud->height = pointcloud_image.get_height_pixels();
+  point_cloud->width = pointcloud_image.get_width_pixels();
+
   const int16_t* point_cloud_buffer = reinterpret_cast<const int16_t*>(pointcloud_image.get_buffer());
   const uint8_t* color_buffer = color_image.get_buffer();
 
@@ -740,6 +744,10 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se
 
   pcd_modifier.resize(point_count);
 
+  // Restore actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1
+  point_cloud->height = pointcloud_image.get_height_pixels();
+  point_cloud->width = pointcloud_image.get_width_pixels();
+
   const int16_t* point_cloud_buffer = reinterpret_cast<const int16_t*>(pointcloud_image.get_buffer());
 
   for (size_t i = 0; i < point_count; i++, ++iter_x, ++iter_y, ++iter_z)

urdf/azure_kinect_macro.urdf.xacro

@@ -0,0 +1,132 @@
+<?xml version="1.0"?>
+<!-- 
+ Copyright (c) Microsoft Corporation. All rights reserved.
+ Licensed under the MIT License.
+ -->
+
+<robot name="azure_kinect" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="azure_kinect" params="prefix:='' parent:='' gazebo:=false *origin">
+
+    <xacro:if value="${parent != ''}">
+      <joint name="${prefix}azure_in_joint" type="fixed">
+        <xacro:insert_block name="origin"/>
+        <parent link="${parent}"/>
+        <child link="${prefix}azure_base"/>
+      </joint>
+    </xacro:if>
+
+    <link name="${prefix}azure_base">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://azure_kinect_ros_driver/meshes/visual.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://azure_kinect_ros_driver/meshes/collision.dae"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="${prefix}camera_base" /> <!-- for compatability with old model -->
+    <link name="${prefix}azure_rgb" />
+    <link name="${prefix}azure_depth" />
+    <link name="${prefix}azure_gyro" />
+
+    <joint name="${prefix}azure_base_to_camera_base" type="fixed">
+      <parent link="${prefix}azure_base" />
+      <child link="${prefix}camera_base" />
+      <origin xyz="0.062 0.0 0.0065" rpy="0 0 0" />
+    </joint>
+
+    <joint name="${prefix}azure_base_to_rgb" type="fixed">
+      <parent link="${prefix}azure_base" />
+      <child link="${prefix}azure_rgb" />
+      <origin xyz="0.0618 -0.032 0.0065" rpy="-${pi/2} 0 -${pi/2}" />
+    </joint>
+
+    <joint name="${prefix}azure_base_to_depth" type="fixed">
+      <parent link="${prefix}azure_base" />
+      <child link="${prefix}azure_depth" />
+      <origin xyz="0.0618 -0.0005 0.0065" rpy="-${96/180*pi} 0 -${pi/2}" />
+    </joint>
+
+    <joint name="${prefix}azure_base_to_gyro" type="fixed">
+      <parent link="${prefix}azure_base" />
+      <child link="${prefix}azure_gyro" />
+      <origin xyz="0.0618 -0.0005 0.0065" rpy="0 ${pi} 0" />
+    </joint>
+
+    <xacro:if value="${gazebo}">
+      <!-- Create a dummy frame for Gazebo, as the depth image is wrongly flipped and rotated
+           (see #243 https://github.com/ros-simulation/gazebo_ros_pkgs/issues/243) -->
+      <link name="${prefix}azure_gazebo_depth" />
+
+      <joint name="${prefix}azure_base_to_gazebo_depth" type="fixed">
+        <parent link="${prefix}azure_base" />
+        <child link="${prefix}azure_gazebo_depth"/>
+        <origin xyz="0.0618 -0.0005 0.0065" rpy="0 ${6/180*pi} 0"/>
+      </joint>
+
+      <gazebo reference="${prefix}azure_gazebo_depth">
+        <sensor name="${prefix}azure_gazebo_sensor" type="depth">
+          <!--
+            NFOV unbinned:        0, 5, 15, 30 FPS
+            NFOV 2x2 binned (SW): 0, 5, 15, 30 FPS
+            WFOV 2x2 binned:      0, 5, 15, 30 FPS
+            WFOV unbinned:        0, 5, 15 FPS
+
+            Note: Frame rate also needs to be changed in gazebo plugin setting (line 107)
+          -->
+          <update_rate>15</update_rate>
+          <camera>
+            <!--
+              NFOV: 1.309 rad (75 deg)
+              WFOV: 2.0944 rad (120 deg)
+            -->
+            <horizontal_fov>2.0944</horizontal_fov>
+            <image>
+              <!--
+                NFOV unbinned:         640x576
+                NFOV 2x2 binned (SW):  320x288
+                WFOV 2x2 binned:       512x512
+                WFOV unbinned:         1024x1024
+              -->
+              <width>512</width>
+              <height>512</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <!--
+                NFOV unbinned:         0.5 - 3.86 m
+                NFOV 2x2 binned (SW):  0.5 - 5.46 m
+                WFOV 2x2 binned:       0.25 - 2.88 m
+                WFOV unbinned:         0.25 - 2.21 m
+              -->
+              <near>0.25</near>
+              <far>2.88</far>
+            </clip>
+          </camera>
+          <plugin filename="libgazebo_ros_openni_kinect.so" name="${prefix}azure_camera_controller">
+            <alwaysOn>true</alwaysOn>
+            <updateRate>15</updateRate>
+            <imageTopicName>rgb/image_raw</imageTopicName>
+            <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
+            <depthImageTopicName>depth/image_raw</depthImageTopicName>
+            <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
+            <pointCloudTopicName>depth/points</pointCloudTopicName>
+            <frameName>${prefix}azure_depth</frameName>
+            <distortion_k1>0.0</distortion_k1>
+            <distortion_k2>0.0</distortion_k2>
+            <distortion_k3>0.0</distortion_k3>
+            <distortion_t1>0.0</distortion_t1>
+            <distortion_t2>0.0</distortion_t2>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:if>
+
+  </xacro:macro>
+</robot>

urdf/azure_kinect_standalone.urdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<!--
+ Copyright (c) Microsoft Corporation. All rights reserved.
+ Licensed under the MIT License.
+ -->
+
+<robot name="azure_kinect" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find azure_kinect_ros_driver)/urdf/azure_kinect_macro.urdf.xacro" />
+
+    <xacro:azure_kinect prefix="" parent="" gazebo="false">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:azure_kinect>
+</robot>