60 #include <visp3/core/vpDebug.h> 61 #include <visp3/core/vpDisplay.h> 62 #include <visp3/core/vpImage.h> 63 #include <visp3/core/vpImagePoint.h> 64 #include <visp3/io/vpImageIo.h> 65 #include <visp3/sensor/vpSickLDMRS.h> 66 #ifdef VISP_HAVE_MODULE_GUI 67 #include <visp3/gui/vpDisplayGTK.h> 68 #include <visp3/gui/vpDisplayX.h> 70 #include <visp3/core/vpIoTools.h> 71 #include <visp3/io/vpParseArgv.h> 72 #include <visp3/sensor/vp1394TwoGrabber.h> 74 #if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \ 75 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK)) 78 static int layerToDisplay = 0xF;
80 double time_offset = 0;
81 #ifdef VISP_HAVE_PTHREAD 82 pthread_mutex_t shm_mutex;
84 std::string output_path;
86 void *laser_display_and_save_loop(
void *);
87 void *laser_acq_loop(
void *);
88 void *camera_acq_and_display_loop(
void *);
90 void *laser_display_and_save_loop(
void *)
94 unsigned int width = map.getWidth();
95 unsigned int height = map.getHeight();
101 char filename[FILENAME_MAX];
102 std::ofstream fdscan;
105 for (
int layer = 0; layer < 4; layer++) {
123 #ifdef VISP_HAVE_MODULE_GUI 124 #if defined VISP_HAVE_X11 126 #elif defined VISP_HAVE_GTK 129 display->init(map, 10, 10,
"Laser scan");
132 unsigned int iter = 0;
135 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 139 #ifdef VISP_HAVE_PTHREAD 140 pthread_mutex_lock(&shm_mutex);
142 for (
int layer = 0; layer < 4; layer++)
143 laserscan[layer] = shm_laserscan[layer];
144 #ifdef VISP_HAVE_PTHREAD 145 pthread_mutex_unlock(&shm_mutex);
152 for (
int layer = 0; layer < 4; layer++) {
153 if (!((0x1 << layer) & layerToDisplay)) {
154 std::cout <<
"Layer " << layer + 1 <<
" is not displayed" << std::endl;
158 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
162 sprintf(filename,
"%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
163 fdscan.open(filename);
166 fdscan <<
"# Scan layer [1 to 4] : " << layer + 1 << std::endl
167 <<
"# Start timestamp (s) : " << laserscan[layer].
getStartTimestamp() - time_offset << std::endl
168 <<
"# End timestamp (s) : " << laserscan[layer].
getEndTimestamp() - time_offset << std::endl
169 <<
"# Data : \"radial distance (m)\" \"horizontal angle " 170 "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z " 176 double resolution = 5;
180 for (
unsigned int i = 0; i < pointsLayer.size(); i++) {
185 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 190 fdscan << p << std::endl;
197 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 208 void *laser_acq_loop(
void *)
210 std::string ip =
"131.254.12.119";
217 unsigned int iter = 0;
220 if (laser.
measure(laserscan) ==
false)
223 #ifdef VISP_HAVE_PTHREAD 224 pthread_mutex_lock(&shm_mutex);
226 for (
int layer = 0; layer < 4; layer++)
227 shm_laserscan[layer] = laserscan[layer];
228 #ifdef VISP_HAVE_PTHREAD 229 pthread_mutex_unlock(&shm_mutex);
239 void *camera_acq_and_display_loop(
void *)
241 #ifdef VISP_HAVE_DC1394 259 #ifdef VISP_HAVE_MODULE_GUI 260 #if defined VISP_HAVE_X11 262 #elif defined VISP_HAVE_GTK 265 display->init(Q, 320, 10,
"Camera");
269 std::ofstream fdimage_ts;
271 std::string filename = output_path +
"/image_timestamp.txt";
272 fdimage_ts.open(filename.c_str());
273 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
276 char filename[FILENAME_MAX];
280 dc1394video_frame_t *frame = g.
dequeue(I, timestamp,
id);
282 double image_timestamp = timestamp / 1000000. - time_offset;
283 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
286 sprintf(filename,
"%s/image%04u.png", output_path.c_str(), iter);
288 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
290 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 308 int main(
int argc,
const char **argv)
311 output_path =
"data";
318 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
324 vpParseArgv::vpArgvInfo argTable[] = {
326 "The layer to display:\n" 327 "\t\t. 0x1 for layer 1.\n" 328 "\t\t. 0x2 for layer 2.\n" 329 "\t\t. 0x4 for layer 3.\n" 330 "\t\t. 0x8 for layer 4.\n" 331 "\t\tTo display all the layers you should set 0xF value."},
334 "Display one or more measured layers form a Sick LD-MRS laser " 342 return (EXIT_FAILURE);
346 #ifdef VISP_HAVE_PTHREAD 347 pthread_t thread_camera_acq;
348 pthread_t thread_laser_acq;
349 pthread_t thread_laser_display;
350 pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
351 pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
352 pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
353 pthread_join(thread_camera_acq, 0);
354 pthread_join(thread_laser_acq, 0);
355 pthread_join(thread_laser_display, 0);
360 std::cout <<
"Catch an exception: " << e << std::endl;
365 #elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK)) 368 std::cout <<
"You do not have X11, or GTK functionalities to display images..." << std::endl;
369 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
370 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
371 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
372 std::cout <<
"- Install GTK, configure again ViSP using cmake and build again this example" << std::endl;
375 #else // #ifdef UNIX and display 379 std::cout <<
"This example is only working on unix-like platforms \n" 380 <<
"since the Sick LD-MRS driver was not ported to Windows." << std::endl;
384 #endif // #ifdef UNIX
End of the argument list.
void getNumCameras(unsigned int &ncameras) const
Class that defines generic functionnalities for display.
static void close(vpImage< unsigned char > &I)
Argument is for help displaying.
double getRadialDist() const
VISP_EXPORT double measureTimeSecond()
Class to define RGB colors available for display functionnalities.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
error that can be emited by ViSP classes.
std::vector< vpScanPoint > getScanPoints()
static const vpColor green
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Print an error message if an option is not in the argument list.
Class that defines a single laser scanner point.
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void enqueue(dc1394video_frame_t *frame)
void quarterSizeImage(vpImage< Type > &res) const
No abrevation. Print an error message if an option is abrevated (ie "-i" in place of "-int" which is ...
bool setup(const std::string &ip, int port)
double getStartTimestamp()
Driver for the Sick LD-MRS laser scanner.
dc1394video_frame_t * dequeue()
bool measure(vpLaserScan laserscan[4])
No default options like -help.
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static const vpColor yellow
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Argument is associated to an int.
void setIpAddress(std::string ip_address)
static const vpColor blue