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/vpDisplayGDI.h> 68 #include <visp3/gui/vpDisplayGTK.h> 69 #include <visp3/gui/vpDisplayX.h> 71 #include <visp3/core/vpIoTools.h> 72 #include <visp3/io/vpParseArgv.h> 73 #include <visp3/sensor/vp1394TwoGrabber.h> 75 #if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \ 76 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 79 static int layerToDisplay = 0xF;
81 double time_offset = 0;
82 #ifdef VISP_HAVE_PTHREAD 83 pthread_mutex_t shm_mutex;
85 std::string output_path;
87 void *laser_display_and_save_loop(
void *);
88 void *laser_acq_loop(
void *);
89 void *camera_acq_and_display_loop(
void *);
91 void *laser_display_and_save_loop(
void *)
95 unsigned int width = map.getWidth();
96 unsigned int height = map.getHeight();
102 char filename[FILENAME_MAX];
103 std::ofstream fdscan;
106 for (
int layer = 0; layer < 4; layer++) {
124 #ifdef VISP_HAVE_MODULE_GUI 125 #if defined VISP_HAVE_X11 127 #elif defined VISP_HAVE_GDI 129 #elif defined VISP_HAVE_GTK 132 display->init(map, 10, 10,
"Laser scan");
135 unsigned int iter = 0;
138 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 142 #ifdef VISP_HAVE_PTHREAD 143 pthread_mutex_lock(&shm_mutex);
145 for (
int layer = 0; layer < 4; layer++)
146 laserscan[layer] = shm_laserscan[layer];
147 #ifdef VISP_HAVE_PTHREAD 148 pthread_mutex_unlock(&shm_mutex);
155 for (
int layer = 0; layer < 4; layer++) {
156 if (!((0x1 << layer) & layerToDisplay)) {
157 std::cout <<
"Layer " << layer + 1 <<
" is not displayed" << std::endl;
161 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
165 sprintf(filename,
"%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
166 fdscan.open(filename);
169 fdscan <<
"# Scan layer [1 to 4] : " << layer + 1 << std::endl
170 <<
"# Start timestamp (s) : " << laserscan[layer].
getStartTimestamp() - time_offset << std::endl
171 <<
"# End timestamp (s) : " << laserscan[layer].
getEndTimestamp() - time_offset << std::endl
172 <<
"# Data : \"radial distance (m)\" \"horizontal angle " 173 "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z " 179 double resolution = 5;
183 for (
unsigned int i = 0; i < pointsLayer.size(); i++) {
188 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 193 fdscan << p << std::endl;
200 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 211 void *laser_acq_loop(
void *)
213 std::string ip =
"131.254.12.119";
220 unsigned int iter = 0;
223 if (laser.
measure(laserscan) ==
false)
226 #ifdef VISP_HAVE_PTHREAD 227 pthread_mutex_lock(&shm_mutex);
229 for (
int layer = 0; layer < 4; layer++)
230 shm_laserscan[layer] = laserscan[layer];
231 #ifdef VISP_HAVE_PTHREAD 232 pthread_mutex_unlock(&shm_mutex);
242 void *camera_acq_and_display_loop(
void *)
244 #ifdef VISP_HAVE_DC1394 262 #ifdef VISP_HAVE_MODULE_GUI 263 #if defined VISP_HAVE_X11 265 #elif defined VISP_HAVE_GDI 267 #elif defined VISP_HAVE_GTK 270 display->init(Q, 320, 10,
"Camera");
274 std::ofstream fdimage_ts;
276 std::string filename = output_path +
"/image_timestamp.txt";
277 fdimage_ts.open(filename.c_str());
278 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
281 char filename[FILENAME_MAX];
285 dc1394video_frame_t *frame = g.
dequeue(I, timestamp,
id);
287 double image_timestamp = timestamp / 1000000. - time_offset;
288 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
291 sprintf(filename,
"%s/image%04u.png", output_path.c_str(), iter);
293 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
295 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) 313 int main(
int argc,
const char **argv)
316 output_path =
"data";
323 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
329 vpParseArgv::vpArgvInfo argTable[] = {
331 "The layer to display:\n" 332 "\t\t. 0x1 for layer 1.\n" 333 "\t\t. 0x2 for layer 2.\n" 334 "\t\t. 0x4 for layer 3.\n" 335 "\t\t. 0x8 for layer 4.\n" 336 "\t\tTo display all the layers you should set 0xF value."},
339 "Display one or more measured layers form a Sick LD-MRS laser " 347 return (EXIT_FAILURE);
351 #ifdef VISP_HAVE_PTHREAD 352 pthread_t thread_camera_acq;
353 pthread_t thread_laser_acq;
354 pthread_t thread_laser_display;
355 pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
356 pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
357 pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
358 pthread_join(thread_camera_acq, 0);
359 pthread_join(thread_laser_acq, 0);
360 pthread_join(thread_laser_display, 0);
365 std::cout <<
"Catch an exception: " << e << std::endl;
370 #else // #ifdef UNIX and display 374 std::cout <<
"This example is only working on UNIX platforms \n" 375 <<
"since the Sick LD-MRS driver was not ported to Windows." << std::endl;
379 #endif // #ifdef UNIX
void getNumCameras(unsigned int &ncameras) const
Class that defines generic functionnalities for display.
static void close(vpImage< unsigned char > &I)
double getRadialDist() const
VISP_EXPORT double measureTimeSecond()
Display for windows using GDI (available on any windows 32 platform).
Class to define 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)
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 ...
static void write(const vpImage< unsigned char > &I, const std::string &filename)
void set_i(const double ii)
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
void set_j(const double jj)
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])
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 void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static const vpColor yellow
void setIpAddress(std::string ip_address)
static const vpColor blue