Example that shows how to acquire Sick LD-MRS laser measurements.This example shows by multithreading how to:
Concerning the laser, layer 1 is displayed in red, layer 2 in green, l ayer 3 in blue and layer 4 in yellow.
Thanks to the -layer command line option, this example allows to select the layers to proceed.
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/sensor/vpSickLDMRS.h>
#ifdef VISP_HAVE_MODULE_GUI
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
#endif
#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \
(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
static int save = 0;
static int layerToDisplay = 0xF;
double time_offset = 0;
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_t shm_mutex;
#endif
std::string output_path;
void *laser_display_and_save_loop(void *);
void *laser_acq_loop(void *);
void *camera_acq_and_display_loop(void *);
void *laser_display_and_save_loop(void *)
{
map = 0;
unsigned int width = map.getWidth();
unsigned int height = map.getHeight();
char filename[FILENAME_MAX];
std::ofstream fdscan;
for (int layer = 0; layer < 4; layer++) {
switch (layer) {
case 0:
break;
case 1:
break;
case 2:
break;
case 3:
break;
}
}
#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
#elif defined VISP_HAVE_GTK
#endif
display->init(map, 10, 10, "Laser scan");
#endif
unsigned int iter = 0;
for (;;) {
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_lock(&shm_mutex);
#endif
for (int layer = 0; layer < 4; layer++)
laserscan[layer] = shm_laserscan[layer];
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock(&shm_mutex);
#endif
for (int layer = 0; layer < 4; layer++) {
if (!((0x1 << layer) & layerToDisplay)) {
std::cout << "Layer " << layer + 1 << " is not displayed" << std::endl;
continue;
}
std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
if (save) {
sprintf(filename, "%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
fdscan.open(filename);
fdscan << "# Scan layer [1 to 4] : " << layer + 1 << std::endl
<<
"# Start timestamp (s) : " << laserscan[layer].
getStartTimestamp() - time_offset << std::endl
<<
"# End timestamp (s) : " << laserscan[layer].
getEndTimestamp() - time_offset << std::endl
<< "# Data : \"radial distance (m)\" \"horizontal angle "
"(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z "
"(m)\""
<< std::endl;
}
double resolution = 5;
for (unsigned int i = 0; i < pointsLayer.size(); i++) {
p = pointsLayer[i];
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
if (save) {
fdscan << p << std::endl;
}
}
if (save) {
fdscan.close();
}
}
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
iter++;
}
delete display;
return NULL;
}
void *laser_acq_loop(void *)
{
std::string ip = "131.254.12.119";
unsigned int iter = 0;
for (;;) {
if (laser.
measure(laserscan) ==
false)
continue;
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_lock(&shm_mutex);
#endif
for (int layer = 0; layer < 4; layer++)
shm_laserscan[layer] = laserscan[layer];
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock(&shm_mutex);
#endif
iter++;
}
return NULL;
}
void *camera_acq_and_display_loop(void *)
{
#ifdef VISP_HAVE_DC1394
try {
return NULL;
#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
#elif defined VISP_HAVE_GTK
#endif
display->init(Q, 320, 10, "Camera");
#endif
std::ofstream fdimage_ts;
if (save) {
std::string filename = output_path + "/image_timestamp.txt";
fdimage_ts.open(filename.c_str());
fdimage_ts << "# [image name] [time stamp in second]" << std::endl;
}
unsigned iter = 0;
char filename[FILENAME_MAX];
uint64_t timestamp;
uint32_t id;
for (;;) {
dc1394video_frame_t *frame = g.
dequeue(I, timestamp,
id);
double image_timestamp = timestamp / 1000000. - time_offset;
std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
if (save) {
sprintf(filename, "%s/image%04u.png", output_path.c_str(), iter);
fdimage_ts << filename << " " << image_timestamp << std::endl;
}
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
iter++;
}
delete display;
if (save) {
}
} catch (...) {
}
#endif
return NULL;
}
int main(int argc, const char **argv)
{
try {
output_path = "data";
try {
} catch (...) {
std::cout << "Cannot create " << output_path << " directory" << std::endl;
return EXIT_FAILURE;
}
}
vpParseArgv::vpArgvInfo argTable[] = {
"The layer to display:\n"
"\t\t. 0x1 for layer 1.\n"
"\t\t. 0x2 for layer 2.\n"
"\t\t. 0x4 for layer 3.\n"
"\t\t. 0x8 for layer 4.\n"
"\t\tTo display all the layers you should set 0xF value."},
"Display one or more measured layers form a Sick LD-MRS laser "
"scanner."},
return (EXIT_FAILURE);
}
#ifdef VISP_HAVE_PTHREAD
pthread_t thread_camera_acq;
pthread_t thread_laser_acq;
pthread_t thread_laser_display;
pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
pthread_join(thread_camera_acq, 0);
pthread_join(thread_laser_acq, 0);
pthread_join(thread_laser_display, 0);
#endif
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
int main()
{
std::cout << "You do not have X11, or GTK functionalities to display images..." << std::endl;
std::cout << "Tip if you are on a unix-like system:" << std::endl;
std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
std::cout << "Tip if you are on a windows-like system:" << std::endl;
std::cout << "- Install GTK, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#else // #ifdef UNIX and display
int main()
{
std::cout << "This example is only working on unix-like platforms \n"
<< "since the Sick LD-MRS driver was not ported to Windows." << std::endl;
return EXIT_SUCCESS;
}
#endif // #ifdef UNIX