Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
tutorial-grabber-multiple-realsense.cpp
1 
2 #include <visp3/core/vpImage.h>
3 #include <visp3/gui/vpDisplayGDI.h>
4 #include <visp3/gui/vpDisplayOpenCV.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/sensor/vpRealSense.h>
7 #include <visp3/sensor/vpRealSense2.h>
8 
13 int main(int argc, char **argv)
14 {
15 #if defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0)) \
16  && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
17  std::vector<std::pair<std::string, std::string> > type_serial_nb;
18  std::vector<bool> cam_found;
19 
20  for (int i = 0; i < argc; i++) {
21  if (std::string(argv[i]) == "--T265") {
22  type_serial_nb.push_back(std::make_pair("T265", std::string(argv[i + 1])));
23  }
24  else if (std::string(argv[i]) == "--D435") {
25  type_serial_nb.push_back(std::make_pair("D435", std::string(argv[i + 1])));
26  }
27  else if (std::string(argv[i]) == "--SR300") {
28  type_serial_nb.push_back(std::make_pair("SR300", std::string(argv[i + 1])));
29  }
30  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
31  std::cout << "\nUsage: " << argv[0]
32  << " [--T265 <serial number>] [--D435 <serial number>] [--SR300 <serial number>]\n"
33  << "\nExample to use 2 T265 cameras:\n"
34  << " " << argv[0] << " --T265 11622110511 --T265 11622110433 \n"
35  << "\nExample to use 1 T265 and 1 D435 cameras:\n"
36  << " " << argv[0] << " --T265 11622110511 --D435 752112077408 \n"
37  << "\nExample to use 2 T265 and 1 D435 cameras:\n"
38  << " " << argv[0] << " --T265 11622110511 --T265 11622110433 --D435 752112070408 \n"
39  << std::endl;
40  return EXIT_SUCCESS;
41  }
42  }
43 
44  rs2::config T265_cfg, D435_cfg;
45  std::vector<vpRealSense2> g(type_serial_nb.size());
46  std::vector<vpImage<unsigned char> > I(type_serial_nb.size());
47 
48 #ifdef VISP_HAVE_X11
49  std::vector<vpDisplayX> d(type_serial_nb.size());
50 #elif defined(VISP_HAVE_GDI)
51  std::vector<vpDisplayGDI> d(type_serial_nb.size());
52 #elif defined(HAVE_OPENCV_HIGHGUI)
53  std::vector<vpDisplayOpenCV> d(type_serial_nb.size());
54 #else
55  std::cout << "No image viewer is available..." << std::endl;
56 #endif
57 
58  bool clicked = false;
59 
60  for (size_t i = 0; i < type_serial_nb.size(); i++) {
61  std::cout << "Opening " << type_serial_nb[i].first << " with ID: " << type_serial_nb[i].second << "." << std::endl;
62  if (type_serial_nb[i].first == "T265") { // T265.
63  T265_cfg.enable_device(type_serial_nb[i].second);
64  T265_cfg.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
65  T265_cfg.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
66  cam_found.push_back(g[i].open(T265_cfg));
67  if (!cam_found.back()) {
68  std::cout << "Device with ID: " << type_serial_nb[i].second << " not found." << std::endl;
69  }
70  }
71  else { // D435 or SR300
72  D435_cfg.enable_device(type_serial_nb[i].second);
73  D435_cfg.disable_stream(RS2_STREAM_DEPTH);
74  D435_cfg.disable_stream(RS2_STREAM_INFRARED);
75  D435_cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
76  cam_found.push_back(g[i].open(D435_cfg));
77  if (!cam_found.back()) {
78  std::cout << "Device with ID: " << type_serial_nb[i].second << " not found." << std::endl;
79  }
80  }
81  }
82 
83  while (true) {
84  for (size_t i = 0; i < type_serial_nb.size(); i++) {
85  if (cam_found[i]) {
86  if (type_serial_nb[i].first == "T265") { // T265.
87  g[i].acquire(&I[i], nullptr, nullptr);
88 
89 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
90  if (!d[i].isInitialised()) {
91  d[i].init(I[i], static_cast<int>(100 * i), static_cast<int>(100 * i), "T265 left image");
92  }
93 #endif
94  }
95 
96  else { // D435.
97  g[i].acquire(I[i]);
98 
99 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
100  if (!d[i].isInitialised()) {
101  d[i].init(I[i], static_cast<int>(100 * i), static_cast<int>(100 * i), type_serial_nb[i].first.c_str());
102  }
103 #endif
104  }
105 
106  vpDisplay::display(I[i]);
107  vpDisplay::flush(I[i]);
108  clicked = vpDisplay::getClick(I[i], false);
109  }
110 
111  if (clicked) {
112  break;
113  }
114  }
115 
116  if (clicked) {
117  break;
118  }
119  }
120 #else
121  (void)argc;
122  (void)argv;
123 #if !(defined(VISP_HAVE_REALSENSE2))
124  std::cout << "Install librealsense version > 2.31.0, configure and build ViSP again to use this example" << std::endl;
125 #endif
126 #if !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
127  std::cout << "librealsense is detected but its version is too old. Install librealsense version > 2.31.0, configure "
128  "and build ViSP again to use this example"
129  << std::endl;
130 #endif
131 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
132  std::cout << "This tutorial should be built with c++11 support" << std::endl;
133 #endif
134 #endif
135  return EXIT_SUCCESS;
136 }
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)