From f69749ff68cf0797f850fbf96ef3ddab59b1cb4d Mon Sep 17 00:00:00 2001
From: Mark D Horn <mark.d.horn@intel.com>
Date: Wed, 26 Oct 2016 18:34:39 -0700
Subject: [PATCH] Fix ROS Camera Tests

Enabling ZR300 incorrectly enabled IMU and FishEye camera tests
to all of the cameras. Change default to disable for these tests
and enable only for ZR300.

Correct ZR300 to only check for 1st coefficient in FishEye distortion model.
---
 realsense_camera/test/camera_core.cpp            | 10 ++++++----
 realsense_camera/test/camera_core.h              |  4 ++--
 realsense_camera/test/zr300_nodelet_default.test | 14 +++++++++-----
 3 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/realsense_camera/test/camera_core.cpp b/realsense_camera/test/camera_core.cpp
index c3c5d50071..b241edbcf0 100644
--- a/realsense_camera/test/camera_core.cpp
+++ b/realsense_camera/test/camera_core.cpp
@@ -608,6 +608,7 @@ TEST(RealsenseTests, testFisheyeStream)
 {
   if (g_enable_fisheye)
   {
+    ROS_INFO_STREAM("RealSense Camera - fisheye_avg: " << g_fisheye_avg);
     EXPECT_TRUE(g_fisheye_avg > 0);
     EXPECT_TRUE(g_fisheye_recv);
   }
@@ -619,7 +620,7 @@ TEST(RealsenseTests, testFisheyeStream)
 
 TEST(RealsenseTests, testFisheyeCameraInfo)
 {
-  if (g_enable_imu)
+  if (g_enable_fisheye)
   {
     EXPECT_EQ(g_width_recv[RS_STREAM_FISHEYE], g_caminfo_width_recv[RS_STREAM_FISHEYE]);
     EXPECT_EQ(g_height_recv[RS_STREAM_FISHEYE], g_caminfo_height_recv[RS_STREAM_FISHEYE]);
@@ -645,11 +646,12 @@ TEST(RealsenseTests, testFisheyeCameraInfo)
     EXPECT_TRUE(g_caminfo_projection_recv[RS_STREAM_FISHEYE][10] != 0.0);
     EXPECT_EQ(g_caminfo_projection_recv[RS_STREAM_FISHEYE][11], 0.0);
 
-    // SR300 cameras have Fisheye distortion parameters
-    if (g_camera_type == "SR300")
+    // ZR300 cameras have Fisheye distortion parameters
+    // Only the first coefficient is used/valid
+    if (g_camera_type == "ZR300")
     {
       bool any_are_zero = false;
-      for (unsigned int i = 0; i < 5; i++)
+      for (unsigned int i = 0; i < 1; i++)
       {
         if (g_fisheye_caminfo_D_recv[i] == 0.0)
         {
diff --git a/realsense_camera/test/camera_core.h b/realsense_camera/test/camera_core.h
index ca239cfb3e..9fd3e3fdbf 100644
--- a/realsense_camera/test/camera_core.h
+++ b/realsense_camera/test/camera_core.h
@@ -71,8 +71,8 @@ uint32_t g_infrared1_step_exp; // Expected infrared1 step.
 
 bool g_enable_color = true;
 bool g_enable_depth = true;
-bool g_enable_fisheye = true;
-bool g_enable_imu = true;
+bool g_enable_fisheye = false;
+bool g_enable_imu = false;
 bool g_enable_pointcloud = false;
 
 std::string g_depth_encoding_exp; // Expected depth encoding.
diff --git a/realsense_camera/test/zr300_nodelet_default.test b/realsense_camera/test/zr300_nodelet_default.test
index d6adb40c2b..ab5f826fbb 100644
--- a/realsense_camera/test/zr300_nodelet_default.test
+++ b/realsense_camera/test/zr300_nodelet_default.test
@@ -1,14 +1,18 @@
 <launch>
-  <arg name="camera"       value="camera" />
-  <arg name="camera_type"  default="ZR300" />
+  <arg name="camera"          value="camera" />
+  <arg name="camera_type"     default="ZR300" />
+  <arg name="enable_fisheye"  default="true" />
+  <arg name="enable_imu"      default="true" />
 
   <!-- Start camera -->
   <include file="$(find realsense_camera)/launch/zr300_nodelet_default.launch">
-    <arg name="camera"      value="$(arg camera)" />
-    <arg name="camera_type" value="$(arg camera_type)" />
+    <arg name="camera"          value="$(arg camera)" />
+    <arg name="camera_type"     value="$(arg camera_type)" />
   </include>
 
   <!-- Start test -->
   <test pkg="realsense_camera" type="tests_camera_core" test-name="realsense_camera_test"
-    args="camera_type $(arg camera_type)" />
+    args="camera_type $(arg camera_type)
+    enable_fisheye $(arg enable_fisheye)
+    enable_imu $(arg enable_imu)" />
 </launch>