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