From ba5939013857942ea6c470b6dae2d288996cef26 Mon Sep 17 00:00:00 2001
From: Souriya Trinh <souriya.trinh+github@gmail.com>
Date: Sun, 14 Apr 2024 14:26:00 +0200
Subject: [PATCH] Fix vpRealSense2::getProductLine(). Otherwise, "Frame didn't
 arrive within 15000" error occurs with
 example/device/framegrabber/grabRealSense2.cpp code.

---
 .../include/visp3/sensor/vpRealSense2.h       |  2 +
 .../src/rgb-depth/realsense/vpRealSense2.cpp  | 48 +++++++++++++------
 .../rgb-depth/testRealSense2_D435_opencv.cpp  |  2 +-
 3 files changed, 36 insertions(+), 16 deletions(-)

diff --git a/modules/sensor/include/visp3/sensor/vpRealSense2.h b/modules/sensor/include/visp3/sensor/vpRealSense2.h
index 6cff43fae8..83efbdbced 100644
--- a/modules/sensor/include/visp3/sensor/vpRealSense2.h
+++ b/modules/sensor/include/visp3/sensor/vpRealSense2.h
@@ -363,6 +363,8 @@ class VISP_EXPORT vpRealSense2
 
   std::string getProductLine();
 
+  std::string getSensorInfo();
+
   vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index = -1) const;
 
   bool open(const rs2::config &cfg = rs2::config());
diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp
index e4536ec3a3..758a60963f 100644
--- a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp
+++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp
@@ -1419,21 +1419,23 @@ bool vpRealSense2::open(const rs2::config &cfg, std::function<void(rs2::frame)>
 std::string vpRealSense2::getProductLine()
 {
 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
-  if (!m_init) { // If pipe is not already created, create it. Otherwise, we have already determined the product line
-    rs2::pipeline *pipe = new rs2::pipeline;
-    rs2::pipeline_profile *pipelineProfile = new rs2::pipeline_profile;
-    *pipelineProfile = pipe->start();
-
-    rs2::device dev = pipelineProfile->get_device();
-
-#if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
-    // Query device product line D400/SR300/L500/T200
-    m_product_line = dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE);
-#endif
-
-    pipe->stop();
-    delete pipe;
-    delete pipelineProfile;
+  // With previous code, example/device/framegrabber/grabRealSense2.cpp does not work with D455
+  // Error: Frame didn't arrive within 15000
+  // Following code from:
+  // https://github.com/IntelRealSense/librealsense/blob/4673a37d981164af8eeb8e296e430fc1427e008d/unit-tests/live/memory/test-extrinsics.cpp#L119
+
+  // Reset product line info
+  m_product_line = "unknown";
+
+  rs2::context ctx;
+  auto list = ctx.query_devices();
+  if (list.size() > 0) {
+    // Only one plugged sensor supported
+    auto dev = list.front();
+    auto sensors = dev.query_sensors();
+    if (dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE)) {
+      m_product_line = dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE);
+    }
   }
 
   return m_product_line;
@@ -1442,6 +1444,22 @@ std::string vpRealSense2::getProductLine()
 #endif
 }
 
+/*!
+ * Alias for the &operator<< operator.
+ * Return sensor information such as:
+ *   - device info
+ *   - supported options
+ *   - stream profiles
+ *   - intrinsics / extrinsics
+ *   - [...]
+ */
+std::string vpRealSense2::getSensorInfo()
+{
+  std::ostringstream oss;
+  oss << *this;
+  return oss.str();;
+}
+
 namespace
 {
 // Helper functions to print information about the RealSense device
diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp
index e2edda80bb..40c5f3e4d4 100644
--- a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp
+++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp
@@ -235,7 +235,7 @@ int main()
 
     chrono.stop();
     time_vector.push_back(chrono.getDurationMs());
-    if (cv::waitKey(5) == 27) {
+    if (cv::waitKey(5) == 27 || cv::waitKey(5) == 113) { // Esc or q
       break;
     }
   }