57#include <visp3/core/vpDebug.h>
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpImage.h>
60#include <visp3/core/vpImagePoint.h>
61#include <visp3/io/vpImageIo.h>
62#include <visp3/sensor/vpSickLDMRS.h>
63#ifdef VISP_HAVE_MODULE_GUI
64#include <visp3/gui/vpDisplayGTK.h>
65#include <visp3/gui/vpDisplayX.h>
67#include <visp3/core/vpIoTools.h>
68#include <visp3/io/vpParseArgv.h>
69#include <visp3/sensor/vp1394TwoGrabber.h>
71#if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \
72 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
75static int layerToDisplay = 0xF;
77double time_offset = 0;
78#ifdef VISP_HAVE_PTHREAD
79pthread_mutex_t shm_mutex;
81std::string output_path;
83void *laser_display_and_save_loop(
void *);
84void *laser_acq_loop(
void *);
85void *camera_acq_and_display_loop(
void *);
87void *laser_display_and_save_loop(
void *)
91 unsigned int width = map.getWidth();
92 unsigned int height = map.getHeight();
98 char filename[FILENAME_MAX];
102 for (
int layer = 0; layer < 4; layer++) {
120#ifdef VISP_HAVE_MODULE_GUI
121#if defined(VISP_HAVE_X11)
123#elif defined(VISP_HAVE_GTK)
126 display->init(map, 10, 10,
"Laser scan");
129 unsigned int iter = 0;
132#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
136#ifdef VISP_HAVE_PTHREAD
137 pthread_mutex_lock(&shm_mutex);
139 for (
int layer = 0; layer < 4; layer++)
140 laserscan[layer] = shm_laserscan[layer];
141#ifdef VISP_HAVE_PTHREAD
142 pthread_mutex_unlock(&shm_mutex);
146 for (
int layer = 0; layer < 4; layer++) {
147 if (!((0x1 << layer) & layerToDisplay)) {
148 std::cout <<
"Layer " << layer + 1 <<
" is not displayed" << std::endl;
152 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
156 snprintf(filename, FILENAME_MAX,
"%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
157 fdscan.open(filename);
160 fdscan <<
"# Scan layer [1 to 4] : " << layer + 1 << std::endl
161 <<
"# Start timestamp (s) : " << laserscan[layer].
getStartTimestamp() - time_offset << std::endl
162 <<
"# End timestamp (s) : " << laserscan[layer].
getEndTimestamp() - time_offset << std::endl
163 <<
"# Data : \"radial distance (m)\" \"horizontal angle "
164 "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z "
170 double resolution = 5;
172 for (
unsigned int i = 0; i < pointsLayer.size(); i++) {
177#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
182 fdscan << p << std::endl;
189#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
200void *laser_acq_loop(
void *)
202 std::string ip =
"131.254.12.119";
211 if (laser.
measure(laserscan) ==
false)
214#ifdef VISP_HAVE_PTHREAD
215 pthread_mutex_lock(&shm_mutex);
217 for (
int layer = 0; layer < 4; layer++)
218 shm_laserscan[layer] = laserscan[layer];
219#ifdef VISP_HAVE_PTHREAD
220 pthread_mutex_unlock(&shm_mutex);
229void *camera_acq_and_display_loop(
void *)
231#ifdef VISP_HAVE_DC1394
249#ifdef VISP_HAVE_MODULE_GUI
250#if defined(VISP_HAVE_X11)
252#elif defined(VISP_HAVE_GTK)
255 display->init(Q, 320, 10,
"Camera");
259 std::ofstream fdimage_ts;
261 std::string filename = output_path +
"/image_timestamp.txt";
262 fdimage_ts.open(filename.c_str());
263 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
266 char filename[FILENAME_MAX];
270 dc1394video_frame_t *frame = g.
dequeue(I, timestamp,
id);
272 double image_timestamp = timestamp / 1000000. - time_offset;
273 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
276 snprintf(filename, FILENAME_MAX,
"%s/image%04u.png", output_path.c_str(), iter);
278 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
280#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
298int main(
int argc,
const char **argv)
301 output_path =
"data";
308 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
314 vpParseArgv::vpArgvInfo argTable[] = {
316 "The layer to display:\n"
317 "\t\t. 0x1 for layer 1.\n"
318 "\t\t. 0x2 for layer 2.\n"
319 "\t\t. 0x4 for layer 3.\n"
320 "\t\t. 0x8 for layer 4.\n"
321 "\t\tTo display all the layers you should set 0xF value."},
324 "Display one or more measured layers form a Sick LD-MRS laser "
332 return (EXIT_FAILURE);
336#ifdef VISP_HAVE_PTHREAD
337 pthread_t thread_camera_acq;
338 pthread_t thread_laser_acq;
339 pthread_t thread_laser_display;
340 pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
341 pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
342 pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
343 pthread_join(thread_camera_acq, 0);
344 pthread_join(thread_laser_acq, 0);
345 pthread_join(thread_laser_display, 0);
350 std::cout <<
"Catch an exception: " << e << std::endl;
355#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
358 std::cout <<
"You do not have X11, or GTK functionalities to display images..." << std::endl;
359 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
360 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
361 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
362 std::cout <<
"- Install GTK, configure again ViSP using cmake and build again this example" << std::endl;
369 std::cout <<
"This example is only working on unix-like platforms \n"
370 <<
"since the Sick LD-MRS driver was not ported to Windows." << std::endl;
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 ...
Definition of the vpImage class member functions.
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])
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()