fix test errors

realsense_examples/launch/rs_demo_t265.launch.py

@@ -0,0 +1,39 @@
+# Copyright (c) 2019 Intel Corporation
+#
+# 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.
+
+# /* Author: Gary Liu */
+import os
+import launch
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    # rviz_config_dir = os.path.join(get_package_share_directory('realsense_examples'), 'config', 'demo_rgbd.rviz')
+    # rviz_node = Node(
+    #     package='rviz2',
+    #     node_executable='rviz2',
+    #     node_name='rviz2',
+    #     output = 'screen',
+    #     arguments=['-d', rviz_config_dir],
+    #     parameters=[{'use_sim_time': 'false'}]
+    #     )
+    rgbd_node = Node(
+        package='realsense_node',
+        node_executable='realsense_node',
+        node_namespace='',
+        output='screen',
+        )
+    return launch.LaunchDescription([
+                                    #  rviz_node,
+                                     rgbd_node])

realsense_examples/launch/rs_t265_and_d400.launch.py

@@ -30,10 +30,10 @@
 def generate_launch_description():
     # config the serial number and base frame id of each camera
     t265_base_frame_id = LaunchConfiguration('base_frame_id', default='t265_link')
-    t265_serial_no = LaunchConfiguration('serial_no', default='845412111144')
+    t265_serial_no = LaunchConfiguration('serial_no', default='845412110563')
 
     rgbd_base_frame_id = LaunchConfiguration('base_frame_id', default='d435_link')
-    rgbd_serial_no = LaunchConfiguration('serial_no', default='819312071869')
+    rgbd_serial_no = LaunchConfiguration('serial_no', default='841612070383')
 
     rviz_config_dir = os.path.join(get_package_share_directory('realsense_examples'), 'config', 'rs_cartographer.rviz')
 

realsense_node/CMakeLists.txt

@@ -91,8 +91,9 @@ ament_target_dependencies(realsense_node
 
 install(TARGETS realsense_node
   DESTINATION lib/${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
+)
+
+install(TARGETS realsense_node
   RUNTIME DESTINATION bin
 )
 

realsense_node/src/realsense_node.cpp

@@ -17,13 +17,8 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor exec;
-
   auto realsense_node = std::make_shared<realsense::RealSenseNodeFactory>();
-
-  exec.add_node(realsense_node);
-
-  exec.spin();
+  rclcpp::spin(realsense_node);
   rclcpp::shutdown();
   return 0;
-}
+}

realsense_ros/src/rs_base.cpp

@@ -37,6 +37,7 @@ RealSenseBase::RealSenseBase(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 
 RealSenseBase::~RealSenseBase()
 {
+  pipeline_.stop();
 }
 
 void RealSenseBase::startPipeline()

realsense_ros/src/rs_factory.cpp

@@ -35,10 +35,7 @@ RealSenseNodeFactory::RealSenseNodeFactory(const std::string & node_name, const
 
 RealSenseNodeFactory::~RealSenseNodeFactory()
 {
-  for(rs2::sensor sensor : dev_.query_sensors()) {
-    sensor.stop();
-    sensor.close();
-  }
+  query_thread_.detach();
 }
 
 void RealSenseNodeFactory::init()
@@ -105,7 +102,7 @@ void RealSenseNodeFactory::changeDeviceCallback(rs2::event_information& info)
   if (info.was_removed(dev_)) {
     RCLCPP_ERROR(this->get_logger(), "The device has been disconnected!");
     rs_node_.release();
-    rs_node_.reset(nullptr);
+    // rs_node_.reset(nullptr);
     dev_ = rs2::device();
   }
   if (!dev_) {

realsense_ros/tests/camera_core.cpp

@@ -28,6 +28,8 @@
 
 void getCameraType()
 {
+
+  rs2::context ctx_;
   auto device_list = ctx_.query_devices();
   rs2::device_list & list = device_list;
   if (0 == list.size()) {
@@ -258,23 +260,32 @@ void imageFisheye2Callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg,
   cv::Mat frame(msg->height, msg->width, encoding2Mat(msg->encoding),
     const_cast<unsigned char *>(msg->data.data()), msg->step);
 
-  double infrared2_total = 0.0;
-  int infrared2_count = 0;
-  cv::Scalar mean = cv::mean(frame);
-  for (auto val : mean.val) {
-    infrared2_total += val;
-    infrared2_count += 1;
+  uchar *fisheye2_data = frame.data;
+  double fisheye2_total = 0.0;
+  int fisheye2_count = 1;
+  for (unsigned int i = 0; i < msg->height * msg->width; i++)
+  {
+    if (*fisheye2_data > 0 && *fisheye2_data < 255)
+    {
+      fisheye2_total += *fisheye2_data;
+      fisheye2_count++;
+    }
+    fisheye2_data++;
   }
-
-  if (infrared2_count > 0) {
-    g_infrared2_avg = infrared2_total / infrared2_count;
+  if (fisheye2_count != 0)
+  {
+    g_fisheye2_avg = static_cast<float>(fisheye2_total / fisheye2_count);
   }
 
+  getMsgInfo(RS2_STREAM_FISHEYE, msg);
+  getCameraInfo(RS2_STREAM_FISHEYE, info_msg);
+
   for (unsigned int i = 0; i < 5; i++)
   {
-    g_fisheye2_caminfo_D_recv[1] = info_msg->d[i];
+    g_fisheye2_caminfo_D_recv[i] = info_msg->d[i];
   }
-  g_infrared2_recv = true;
+
+  g_fisheye2_recv = true;
 }
 void imageAlignDepthCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg)
 {
@@ -649,7 +660,7 @@ TEST(RealsenseTests, testFisheye1CameraInfo)
   {
     EXPECT_EQ(g_width_recv[RS2_STREAM_FISHEYE], g_caminfo_width_recv[RS2_STREAM_FISHEYE]);
     EXPECT_EQ(g_height_recv[RS2_STREAM_FISHEYE], g_caminfo_height_recv[RS2_STREAM_FISHEYE]);
-    EXPECT_STREQ(g_dmodel_recv[RS2_STREAM_FISHEYE].c_str(), "distortion_ftheta");
+    EXPECT_STREQ(g_dmodel_recv[RS2_STREAM_FISHEYE].c_str(), "plumb_bob");
 
     // verify rotation is equal to identity matrix
     for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY) / sizeof(double); i++)
@@ -754,8 +765,8 @@ int main(int argc, char * argv[])
   auto sub_pointcloud = node->create_subscription<sensor_msgs::msg::PointCloud2>("camera/pointcloud", rclcpp::QoS(1), pcCallback);
   auto sub_accel = node->create_subscription<sensor_msgs::msg::Imu>("/camera/accel/sample", rclcpp::QoS(1), accelCallback);
   auto sub_gyro = node->create_subscription<sensor_msgs::msg::Imu>("/camera/gyro/sample", rclcpp::QoS(1), gyroCallback);
-  auto fisheye1_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye1/image_rect_raw", imageFisheye1Callback, "raw", custom_qos);
-  auto fisheye2_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye2/image_rect_raw", imageFisheye2Callback, "raw", custom_qos);
+  auto fisheye1_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye1/image_raw", imageFisheye1Callback, "raw", custom_qos);
+  auto fisheye2_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye2/image_raw", imageFisheye2Callback, "raw", custom_qos);
 
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.add_node(node);

realsense_ros/tests/camera_core.hpp

@@ -55,8 +55,8 @@ int g_depth_height_exp = 480;
 int g_depth_width_exp = 640;
 int g_infrared1_height_exp = 480;
 int g_infrared1_width_exp = 640;
-int g_fisheye1_height_exp = 480;
-int g_fisheye1_width_exp = 640;
+int g_fisheye1_height_exp = 800;
+int g_fisheye1_width_exp = 848;
 
 uint32_t g_depth_step_exp;
 uint32_t g_color_step_exp;
@@ -112,7 +112,6 @@ double g_fisheye2_caminfo_D_recv[5] = {0.0};
 double g_caminfo_rotation_recv[STREAM_COUNT][9] = {{0.0}};
 double g_caminfo_projection_recv[STREAM_COUNT][12] = {{0.0}};
 
-rs2::context ctx_;
 std::string g_dmodel_recv[STREAM_COUNT];
 std::string camera_type;
 

realsense_ros/tests/unittest_function.cpp

@@ -19,37 +19,31 @@
 #include <vector>
 #include <chrono>
 
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#include "opencv2/opencv.hpp"
 #include "realsense/rs_factory.hpp"
-#include "realsense/rs_d435.hpp"
-#include "realsense/rs_d435i.hpp"
-#include "realsense/rs_t265.hpp"
 
 TEST(UnitTestFunction, testLibraryFunctions)
-{
+{  
+  rclcpp::WallRate loop_rate(1);
+
   ASSERT_NO_THROW({
     auto realsense_node = std::make_shared<realsense::RealSenseNodeFactory>();
+    rclcpp::spin_some(realsense_node);
+    loop_rate.sleep();
   });
 }
 
 TEST(UnitTestFunction, testLibraryIncorrectInputs)
 {
-  try 
-  {
-    auto realsense_node = std::make_shared<realsense::RealSenseNodeFactory>("ZR300", "0");
-  }
-  catch (...)
-  {
-    SUCCEED();
-  }
+  ASSERT_THROW({
+      auto realsense_node = std::make_shared<realsense::RealSenseNodeFactory>("ZR300", "0");
+    }, rclcpp::exceptions::InvalidNamespaceError);
 }
 
-int main(int argc, char * argv[])
+int main(int argc, char ** argv)
 {
+  rclcpp::init(argc, argv);
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
 }