Update test

realsense_ros2_camera/CMakeLists.txt

@@ -134,6 +134,12 @@ install(
   DESTINATION include
 )
 
+# Install param files.
+install(DIRECTORY
+  test/param
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 # Install launch files.
 install(DIRECTORY
   launch
@@ -144,21 +150,31 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  set(REALSENSE_DEVICE_PLUGIN FALSE)
-  if(${REALSENSE_DEVICE_PLUGIN})
-    ament_add_gtest(test_api test/test_api.cpp)
-  endif()
-  if(TARGET test_api)
-    target_include_directories(test_api PUBLIC
-      ${${PROJECT_NAME}_INCLUDE_DIRS}
-    )
-    ament_target_dependencies(test_api
-      rclcpp
-      sensor_msgs
-      tf2_ros
-      tf2
-      OpenCV)
-  endif()
+  macro(custom_gtest target)
+    ament_add_gtest(${target} ${ARGN})
+    if(TARGET ${target})
+      target_include_directories(${target} PUBLIC
+        ${${PROJECT_NAME}_INCLUDE_DIRS}
+      )
+      ament_target_dependencies(${target}
+        "rclcpp"
+        "OpenCV"
+        "sensor_msgs"
+        "tf2_ros"
+        "tf2")
+    endif()
+  endmacro()
+
+  custom_gtest(test_api_enableAll
+    "test/test_api_enableAll.cpp"
+    TIMEOUT 100)
+  custom_gtest(test_api_enableColor
+    "test/test_api_enableColor.cpp"
+    TIMEOUT 100)
+  custom_gtest(test_api_enableDepth
+    "test/test_api_enableDepth.cpp"
+    TIMEOUT 100)
+
 endif()
 
 ament_package()

realsense_ros2_camera/launch/realsense_params_enableAll.launch.py

@@ -0,0 +1,32 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    default_yaml = os.path.join(get_package_share_directory('realsense_ros2_camera'), 'param',
+                                'realsense_params_enableAll.yaml')
+    return LaunchDescription([
+        # Realsense Camera
+        launch_ros.actions.Node(
+            package='realsense_ros2_camera', node_executable='realsense_ros2_camera',
+            arguments=['__params:=' + default_yaml],
+            output='screen'),
+
+    ])

realsense_ros2_camera/launch/realsense_params_enableColor.launch.py

@@ -0,0 +1,32 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    default_yaml = os.path.join(get_package_share_directory('realsense_ros2_camera'), 'param',
+                                'realsense_params_enableColor.yaml')
+    return LaunchDescription([
+        # Realsense Camera
+        launch_ros.actions.Node(
+            package='realsense_ros2_camera', node_executable='realsense_ros2_camera',
+            arguments=['__params:=' + default_yaml],
+            output='screen'),
+
+    ])

realsense_ros2_camera/launch/realsense_params_enableDepth.launch.py

@@ -0,0 +1,32 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    default_yaml = os.path.join(get_package_share_directory('realsense_ros2_camera'), 'param',
+                                'realsense_params_enableDepth.yaml')
+    return LaunchDescription([
+        # Realsense Camera
+        launch_ros.actions.Node(
+            package='realsense_ros2_camera', node_executable='realsense_ros2_camera',
+            arguments=['__params:=' + default_yaml],
+            output='screen'),
+
+    ])

realsense_ros2_camera/src/realsense_camera_node.cpp

@@ -68,13 +68,19 @@ const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
 const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
 
 const std::vector<std::vector<stream_index_pair>> IMAGE_STREAMS = {{{DEPTH, INFRA1, INFRA2},
-  {COLOR}, {FISHEYE}}};
+    {COLOR}, {FISHEYE}}};
 
 const std::vector<std::vector<stream_index_pair>> HID_STREAMS = {{GYRO, ACCEL}};
 
+rs2::device _dev;
 inline void signalHandler(int signum)
 {
-  std::cout << strsignal(signum) << "Signal is received! Terminate RealSense Node...\n";
+  std::cout << strsignal(signum) << " Signal is received! Terminate RealSense Node...\n";
+  auto sens = _dev.query_sensors();
+  for (auto it=sens.begin(); it!=sens.end(); it++) {
+    it->stop();
+    it->close();
+  }
   rclcpp::shutdown();
   exit(signum);
 }
@@ -183,15 +189,19 @@ class RealSenseCameraNode : public rclcpp::Node
     // this->get_parameter_or("enable_sync", _sync_frames, SYNC_FRAMES);
     this->get_parameter_or("enable_depth", _enable[DEPTH], ENABLE_DEPTH);
     this->get_parameter_or("enable_aligned_depth", _align_depth, ALIGN_DEPTH);
+    this->get_parameter_or("enable_infra1", _enable[INFRA1], ENABLE_INFRA1);
+    this->get_parameter_or("enable_infra2", _enable[INFRA2], ENABLE_INFRA2);
     if (!_enable[DEPTH]) {
       _pointcloud = false;
       _align_depth = false;
+      _enable[INFRA1] = false;
+      _enable[INFRA2] = false;
     }
     if (_pointcloud || _align_depth) {
       _sync_frames = true;
-    }
-    else
+    } else {
       _sync_frames = false;
+    }
     this->get_parameter("serial_no", _serial_no);
 
     this->get_parameter_or("depth_width", _width[DEPTH], DEPTH_WIDTH);
@@ -201,12 +211,10 @@ class RealSenseCameraNode : public rclcpp::Node
     this->get_parameter_or("infra1_width", _width[INFRA1], INFRA1_WIDTH);
     this->get_parameter_or("infra1_height", _height[INFRA1], INFRA1_HEIGHT);
     this->get_parameter_or("infra1_fps", _fps[INFRA1], INFRA1_FPS);
-    this->get_parameter_or("enable_infra1", _enable[INFRA1], ENABLE_INFRA1);
 
     this->get_parameter_or("infra2_width", _width[INFRA2], INFRA2_WIDTH);
     this->get_parameter_or("infra2_height", _height[INFRA2], INFRA2_HEIGHT);
     this->get_parameter_or("infra2_fps", _fps[INFRA2], INFRA2_FPS);
-    this->get_parameter_or("enable_infra2", _enable[INFRA2], ENABLE_INFRA2);
 
     this->get_parameter_or("color_width", _width[COLOR], COLOR_WIDTH);
     this->get_parameter_or("color_height", _height[COLOR], COLOR_HEIGHT);
@@ -830,9 +838,8 @@ class RealSenseCameraNode : public rclcpp::Node
       c2co_msg.transform.rotation.w = q_c2co.getW();
       _static_tf_broadcaster.sendTransform(c2co_msg);
     }
-
-    if (_enable[DEPTH]) {
 
+    if (_enable[DEPTH]) {
       rs2::stream_profile depth_profile;
       if (!getEnabledProfile(DEPTH, depth_profile)) {
         RCLCPP_ERROR(logger_, "Depth profile not found!");
@@ -1229,7 +1236,6 @@ class RealSenseCameraNode : public rclcpp::Node
 
   rclcpp::Clock _ros_clock;
   std::unique_ptr<rs2::context> _ctx;
-  rs2::device _dev;
 
   std::map<stream_index_pair, std::unique_ptr<rs2::sensor>> _sensors;
 

realsense_ros2_camera/test/param/realsense_params_enableAll.yaml

@@ -0,0 +1,6 @@
+RealSenseCameraNode:
+  ros__parameters:
+    enable_depth: True
+    enable_color: True
+    enable_infra1: True
+    enable_infra2: True

realsense_ros2_camera/test/param/realsense_params_enableColor.yaml

@@ -0,0 +1,6 @@
+RealSenseCameraNode:
+  ros__parameters:
+    enable_depth: False
+    enable_color: True
+    enable_infra1: True
+    enable_infra2: True

realsense_ros2_camera/test/param/realsense_params_enableDepth.yaml

@@ -0,0 +1,6 @@
+RealSenseCameraNode:
+  ros__parameters:
+    enable_depth: True
+    enable_color: False
+    enable_infra1: True
+    enable_infra2: True

realsense_ros2_camera/test/test_api.cpp → realsense_ros2_camera/test/test_api_enableAll.cpp

@@ -28,22 +28,17 @@
 #include <map>
 #include <string>
 
-bool g_enable_color = true;
 bool g_color_recv = false;
 float g_color_avg = 0.0f;
 
-bool g_enable_depth = true;
 bool g_depth_recv = false;
 
-bool g_enable_infrared1 = true;
 bool g_infrared1_recv = false;
 float g_infrared1_avg = 0.0f;
 
-bool g_enable_infrared2 = true;
 bool g_infrared2_recv = false;
 float g_infrared2_avg = 0.0f;
 
-bool g_enable_pc = true;
 bool g_pc_recv = false;
 float g_pc_depth_avg = 0.0f;
 
@@ -185,47 +180,27 @@ void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
 }
 
 TEST(TestAPI, testDepthStream) {
-  if (g_enable_depth) {
-    EXPECT_TRUE(g_depth_recv);
-  } else {
-    EXPECT_FALSE(g_depth_recv);
-  }
+  EXPECT_TRUE(g_depth_recv);
 }
 
 TEST(TestAPI, testColorStream) {
-  if (g_enable_color) {
-    EXPECT_GT(g_color_avg, 0);
-    EXPECT_TRUE(g_color_recv);
-  } else {
-    EXPECT_FALSE(g_color_recv);
-  }
+  EXPECT_GT(g_color_avg, 0);
+  EXPECT_TRUE(g_color_recv);
 }
 
 TEST(TestAPI, testInfrared1Stream) {
-  if (g_enable_infrared1) {
-    EXPECT_GT(g_infrared1_avg, 0);
-    EXPECT_TRUE(g_infrared1_recv);
-  } else {
-    EXPECT_FALSE(g_infrared1_recv);
-  }
+  EXPECT_GT(g_infrared1_avg, 0);
+  EXPECT_TRUE(g_infrared1_recv);
 }
 
 TEST(TestAPI, testInfrared2Stream) {
-  if (g_enable_infrared2) {
-    EXPECT_GT(g_infrared2_avg, 0);
-    EXPECT_TRUE(g_infrared2_recv);
-  } else {
-    EXPECT_FALSE(g_infrared2_recv);
-  }
+  EXPECT_GT(g_infrared2_avg, 0);
+  EXPECT_TRUE(g_infrared2_recv);
 }
 
 TEST(TestAPI, testPointCloud) {
-  if (g_enable_pc) {
-    EXPECT_GT(g_pc_depth_avg, 0);
-    EXPECT_TRUE(g_pc_recv);
-  } else {
-    EXPECT_FALSE(g_pc_recv);
-  }
+  EXPECT_GT(g_pc_depth_avg, 0);
+  EXPECT_TRUE(g_pc_recv);
 }
 
 TEST(TestAPI, testTF) {
@@ -264,7 +239,7 @@ int main(int argc, char * argv[]) try
   auto sub6 = node->create_subscription<tf2_msgs::msg::TFMessage>("tf_static",
       tfCallback, rmw_qos_profile_default);
 
-  system("realsense_ros2_camera &");
+  system("ros2 launch realsense_ros2_camera realsense_params_enableAll.launch.py &");
 
   rclcpp::WallRate loop_rate(50);
   for (int i = 0; i < 300; ++i) {
@@ -275,7 +250,7 @@ int main(int argc, char * argv[]) try
     loop_rate.sleep();
   }
 
-  system("killall realsense_ros2_camera &");
+  system("killall -s SIGINT realsense_ros2_camera &");
   return RUN_ALL_TESTS();
 } catch (...) {
 }