From 0aa80aca271543b38e0de7f311f2736ddd5db239 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Sat, 6 Jan 2018 08:41:26 -0800
Subject: [PATCH] [description] Generalization. Add unit test using kobuki.

[description][test] Use source permission to pass the tests in install space.
---
 openni_description/CMakeLists.txt             |  9 ++--
 openni_description/model/kinect.urdf.xacro    | 19 ++++-----
 .../model/kinect_gazebo.urdf.xacro            | 42 +++++++++++++++++++
 openni_description/package.xml                |  3 ++
 openni_description/test/sample_kobuki.test    |  4 ++
 .../test/sample_kobuki.urdf.xacro             | 13 ++++++
 openni_description/test/test_openni_desc.py   | 32 ++++++++++++++
 .../test/turtlebot_properties.urdf.xacro      |  2 +-
 8 files changed, 110 insertions(+), 14 deletions(-)
 create mode 100644 openni_description/model/kinect_gazebo.urdf.xacro
 create mode 100644 openni_description/test/sample_kobuki.test
 create mode 100644 openni_description/test/sample_kobuki.urdf.xacro
 create mode 100755 openni_description/test/test_openni_desc.py

diff --git a/openni_description/CMakeLists.txt b/openni_description/CMakeLists.txt
index a3f32fc..add8022 100644
--- a/openni_description/CMakeLists.txt
+++ b/openni_description/CMakeLists.txt
@@ -5,6 +5,9 @@ find_package(catkin REQUIRED)
 
 catkin_package()
 
-install(DIRECTORY model
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
-)
+install(DIRECTORY model DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY test  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_rostest(test/sample_kobuki.test)
+endif ()
diff --git a/openni_description/model/kinect.urdf.xacro b/openni_description/model/kinect.urdf.xacro
index 8700d5c..f62201c 100644
--- a/openni_description/model/kinect.urdf.xacro
+++ b/openni_description/model/kinect.urdf.xacro
@@ -1,13 +1,12 @@
 <?xml version="1.0"?>
-<robot name="sensor_kinect" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
-  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>
+<robot name="sensor_openni" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find openni_description)/model/kinect_gazebo.urdf.xacro" />
   
-  <xacro:property name="kinect_cam_py" value="-0.0125"/>
   <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
-  <xacro:macro name="sensor_kinect" params="parent">
+  <xacro:macro name="sensor_kinect" params="parent cam_px cam_py cam_pz cam_or cam_op cam_oy">
     <joint name="camera_rgb_joint" type="fixed">
-      <origin xyz="${cam_px} ${kinect_cam_py} ${cam_pz}" rpy="${cam_or} ${cam_op} ${cam_oy}"/>
+      <origin xyz="${cam_px} ${cam_py} ${cam_pz}" rpy="${cam_or} ${cam_op} ${cam_oy}"/>
       <parent link="${parent}"/>
       <child link="camera_rgb_frame" />
     </joint>
@@ -21,7 +20,7 @@
     <link name="camera_rgb_optical_frame"/>
 
     <joint name="camera_joint" type="fixed">
-      <origin xyz="-0.031 ${-kinect_cam_py} -0.016" rpy="0 0 0"/>
+      <origin xyz="-0.031 ${-cam_py} -0.016" rpy="0 0 0"/>
       <parent link="camera_rgb_frame"/>
       <child link="camera_link"/>
     </joint>  
@@ -29,7 +28,7 @@
       <visual>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
         <geometry>
-         <mesh filename="package://turtlebot_description/meshes/sensors/kinect.dae"/>
+         <mesh filename="package://openni_description/mesh/kinect.dae"/>
         </geometry>
       </visual>
   	  <collision>
@@ -51,7 +50,7 @@
          files. However, for Gazebo simulation we need them, so we add them here.
          (Hence, don't publish them additionally!) -->
 	<joint name="camera_depth_joint" type="fixed">
-	  <origin xyz="0 ${2 * -kinect_cam_py} 0" rpy="0 0 0" />
+	  <origin xyz="0 ${2 * -cam_py} 0" rpy="0 0 0" />
 	  <parent link="camera_rgb_frame" />
 	  <child link="camera_depth_frame" />
 	</joint>
@@ -65,6 +64,6 @@
 	<link name="camera_depth_optical_frame"/>
 	
 	<!-- Kinect sensor for simulation -->
-	<turtlebot_sim_3dsensor/>
+	<sim_3dsensor_kinect/>
   </xacro:macro>
 </robot>
diff --git a/openni_description/model/kinect_gazebo.urdf.xacro b/openni_description/model/kinect_gazebo.urdf.xacro
new file mode 100644
index 0000000..c6263fe
--- /dev/null
+++ b/openni_description/model/kinect_gazebo.urdf.xacro
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<robot name="kinect_gazebo" xmlns:xacro="http://wiki.ros.org/xacro">
+  <!-- Microsoft Kinect / ASUS Xtion PRO Live for simulation -->
+  <xacro:macro name="sim_3dsensor_kinect">
+    <gazebo reference="camera_link">  
+      <sensor type="depth" name="camera">
+        <always_on>true</always_on>
+        <update_rate>20.0</update_rate>
+        <camera>
+          <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
+          <image>
+            <format>B8G8R8</format>
+            <width>640</width>
+            <height>480</height>
+          </image>
+          <clip>
+            <near>0.05</near>
+            <far>8.0</far>
+          </clip>
+        </camera>
+        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
+          <cameraName>camera</cameraName>
+          <alwaysOn>true</alwaysOn>
+          <updateRate>10</updateRate>
+          <imageTopicName>rgb/image_raw</imageTopicName>
+          <depthImageTopicName>depth/image_raw</depthImageTopicName>
+          <pointCloudTopicName>depth/points</pointCloudTopicName>
+          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
+          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
+          <frameName>camera_depth_optical_frame</frameName>
+          <baseline>0.1</baseline>
+          <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>
+          <pointCloudCutoff>0.4</pointCloudCutoff>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>
diff --git a/openni_description/package.xml b/openni_description/package.xml
index b72f66c..b2d6729 100644
--- a/openni_description/package.xml
+++ b/openni_description/package.xml
@@ -13,4 +13,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <run_depend>urdf</run_depend>
   <run_depend>xacro</run_depend>
+  <test_depend>kobuki_description</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>turtlebot_description</test_depend>
 </package>
diff --git a/openni_description/test/sample_kobuki.test b/openni_description/test/sample_kobuki.test
new file mode 100644
index 0000000..19578eb
--- /dev/null
+++ b/openni_description/test/sample_kobuki.test
@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="xacro" type="xacro" name="kobuki_urdf_generation" args="$(find openni_description)/test/sample_kobuki.urdf.xacro -o sample_kobuki.urdf" />
+  <test pkg="openni_description" type="test_openni_desc.py" test-name="test_openni_descriptioner" />
+</launch>
\ No newline at end of file
diff --git a/openni_description/test/sample_kobuki.urdf.xacro b/openni_description/test/sample_kobuki.urdf.xacro
new file mode 100644
index 0000000..bb66721
--- /dev/null
+++ b/openni_description/test/sample_kobuki.urdf.xacro
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot name="turtlebot" xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_common_library.urdf.xacro" />
+  <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
+  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/hexagons.urdf.xacro" />
+  <xacro:include filename="$(find openni_description)/model/kinect.urdf.xacro" />
+  <xacro:include filename="$(find openni_description)/test/turtlebot_properties.urdf.xacro"/>
+
+  <sensor_kinect parent="base_link" cam_px="${cam_px}" cam_py="${cam_py}" cam_pz="${cam_pz}" cam_or="${cam_or}" cam_op="${cam_op}" cam_oy="${cam_oy}" />  
+  
+  <kobuki />
+  <stack_hexagons parent="base_link" />
+</robot>
diff --git a/openni_description/test/test_openni_desc.py b/openni_description/test/test_openni_desc.py
new file mode 100755
index 0000000..4d3485f
--- /dev/null
+++ b/openni_description/test/test_openni_desc.py
@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+PKG = 'openni_description'
+
+import os
+import subprocess
+import unittest
+
+import rospkg
+
+
+class TestOpenniDescription(unittest.TestCase):
+
+    def setUp(self):
+        pass
+
+    def tearDown(self):
+        True
+
+    def test_urdf_turtlebot(self):
+        """
+        Check if check_urdf command passes with the urdf that is generated in
+        the .test file this test case is called from.
+        """
+        self.assertTrue(os.path.isfile(rospkg.RosPack().get_path('openni_description') + "/test/sample_kobuki.urdf.xacro")
+        print(subprocess.check_output(["rosrun", "xacro", "xacro", rospkg.RosPack().get_path('openni_description') + "/test/sample_kobuki.urdf.xacro", "-o", "./sample_kobuki.urdf"]))
+        self.assertEqual(0, subprocess.call(["check_urdf", "./sample_kobuki.urdf"]))
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'test_openni_description', TestOpenniDescription) 
diff --git a/openni_description/test/turtlebot_properties.urdf.xacro b/openni_description/test/turtlebot_properties.urdf.xacro
index d4c7d0b..d7915ff 100644
--- a/openni_description/test/turtlebot_properties.urdf.xacro
+++ b/openni_description/test/turtlebot_properties.urdf.xacro
@@ -23,7 +23,7 @@
      (currently in development and awaiting feedback)
    -->
 	<xacro:property name="cam_px" value="-0.087" />
-<!--	<xacro:property name="cam_py" value="-0.0125" /> Y varies for different configurations. get sets in sensors_urdf -->
+        <xacro:property name="cam_py" value="-0.0125"/>
 	<xacro:property name="cam_pz" value="0.2870" />
 	<xacro:property name="cam_or" value="0" />
 	<xacro:property name="cam_op" value="0" />