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) {
snprintf(filename, FILENAME_MAX, "%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++;
}
return nullptr;
}
void *laser_acq_loop(void *)
{
std::string ip = "131.254.12.119";
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
}
return nullptr;
}
void *camera_acq_and_display_loop(void *)
{
#ifdef VISP_HAVE_DC1394
try {
return nullptr;
#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) {
snprintf(filename, FILENAME_MAX, "%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++;
}
if (save) {
fdimage_ts.close();
}
} catch (...) {
}
#endif
return nullptr;
}
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, nullptr, &camera_acq_and_display_loop, nullptr);
pthread_create(&thread_laser_acq, nullptr, &laser_acq_loop, nullptr);
pthread_create(&thread_laser_display, nullptr, &laser_display_and_save_loop, nullptr);
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
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
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void enqueue(dc1394video_frame_t *frame)
dc1394video_frame_t * dequeue()
void getNumCameras(unsigned int &ncameras) const
Class to define RGB colors available for display functionalities.
static const vpColor blue
static const vpColor yellow
static const vpColor green
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Class that defines generic functionalities for display.
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void quarterSizeImage(vpImage< Type > &res) const
Implements a laser scan data structure that contains especially the list of scanned points that have ...
double getStartTimestamp()
std::vector< vpScanPoint > getScanPoints()
void setIpAddress(std::string ip_address)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
@ ARGV_NO_DEFAULTS
No default options like -help.
@ ARGV_NO_LEFTOVERS
Print an error message if an option is not in the argument list.
@ ARGV_INT
Argument is associated to an int.
@ ARGV_END
End of the argument list.
@ ARGV_HELP
Argument is for help displaying.
Class that defines a single laser scanner point.
double getRadialDist() const
Driver for the Sick LD-MRS laser scanner.
bool setup(const std::string &ip, int port)
bool measure(vpLaserScan laserscan[4])
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()