Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
SickLDMRS-Process.cpp

Example that shows how to acquire Sick LD-MRS laser measurements.This example shows by multithreading how to:

Warning
For the moment, this example is only working on UNIX platforms since the Sick LD-MRS driver was not ported to Windows.

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.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Sick LD-MRS laser driver.
*
*****************************************************************************/
#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)) && defined(VISP_HAVE_THREADS)
#include <thread>
#include <mutex>
static int save = 0;
static int layerToDisplay = 0xF; // 0xF = 1111 => all the layers are selected
static vpLaserScan shm_laserscan[4];
double time_offset = 0;
std::mutex shm_mutex;
std::string output_path;
void laser_display_and_save_loop()
{
vpImage<unsigned char> map(700, 300);
map = 0;
unsigned int width = map.getWidth();
unsigned int height = map.getHeight();
vpImagePoint O; // Beam origin
O.set_i(height);
O.set_j(width / 2.);
vpColor color[4]; // one color per layer
char filename[FILENAME_MAX];
std::ofstream fdscan;
vpLaserScan laserscan[4];
for (int layer = 0; layer < 4; layer++) {
switch (layer) {
case 0:
color[layer] = vpColor::red;
break;
case 1:
color[layer] = vpColor::green;
break;
case 2:
color[layer] = vpColor::blue;
break;
case 3:
color[layer] = vpColor::yellow;
break;
}
}
vpDisplay *display = nullptr;
#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
shm_mutex.lock();
for (int layer = 0; layer < 4; layer++) {
laserscan[layer] = shm_laserscan[layer];
}
shm_mutex.unlock();
// Parse the four layers
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) {
// Set the scan data filename to store the measures
snprintf(filename, FILENAME_MAX, "%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
fdscan.open(filename);
// Write the file header
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;
}
vpImagePoint E; // Beam echo
double resolution = 5; // 100 pixels = 1 meter - increase this value to
// see better near info
for (unsigned int i = 0; i < pointsLayer.size(); i++) {
p = pointsLayer[i];
E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle()));
E.set_j(width / 2. - resolution * p.getRadialDist() * sin(p.getHAngle()));
// std::cout << "E: " << E << std::endl;
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
vpDisplay::displayLine(map, O, E, color[layer]);
#endif
if (save) {
// Save the measures in the file
fdscan << p << std::endl;
}
}
if (save) {
fdscan.close();
}
}
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
iter++;
// std::cout << "display time: " << vpTime::measureTimeMs() - t1 <<
// std::endl;
}
delete display;
}
void laser_acq_loop()
{
std::string ip = "131.254.12.119";
vpSickLDMRS laser;
laser.setIpAddress(ip);
laser.setup();
vpLaserScan laserscan[4];
for (;;) {
double t1 = vpTime::measureTimeMs();
if (laser.measure(laserscan) == false)
continue;
shm_mutex.lock();
for (int layer = 0; layer < 4; layer++) {
shm_laserscan[layer] = laserscan[layer];
}
shm_mutex.unlock();
std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl;
}
}
void camera_acq_and_display_loop()
{
#ifdef VISP_HAVE_DC1394
try {
// Initialize the firewire framegrabber
vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib
// If no camera found return
if (g.getNumCameras() == 0) {
return;
}
// g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
// g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
vpImage<unsigned char> I; // Create a gray level image container
vpImage<unsigned char> Q; // Create a quarter size gray level image container
g.acquire(I); // Acquire an image
vpDisplay *display = nullptr;
#ifdef VISP_HAVE_MODULE_GUI
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GTK)
#endif
display->init(Q, 320, 10, "Camera");
#endif
// Create a file with image time stamps
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); // Acquire an image
double image_timestamp = timestamp / 1000000. - time_offset;
std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
if (save) {
// Set the image filename
snprintf(filename, FILENAME_MAX, "%s/image%04u.png", output_path.c_str(), iter);
vpImageIo::write(Q, filename);
fdimage_ts << filename << " " << image_timestamp << std::endl;
}
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
#endif
g.enqueue(frame);
iter++;
}
delete display;
if (save) {
fdimage_ts.close();
}
}
catch (...) {
}
#endif
}
int main(int argc, const char **argv)
{
try {
output_path = "data";
// Test if the output path directory exist. If no try to create it
if (vpIoTools::checkDirectory(output_path) == false) {
try {
// Create a directory with name "username"
}
catch (...) {
std::cout << "Cannot create " << output_path << " directory" << std::endl;
return EXIT_FAILURE;
}
}
// Parse the command line to set the variables
vpParseArgv::vpArgvInfo argTable[] = {
{"-layer", vpParseArgv::ARGV_INT, (char *)nullptr, (char *)&layerToDisplay,
"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."},
{"-save", vpParseArgv::ARGV_INT, (char *)nullptr, (char *)&save, "Turn to 1 in order to save data."},
{"-h", vpParseArgv::ARGV_HELP, (char *)nullptr, (char *)nullptr,
"Display one or more measured layers form a Sick LD-MRS laser "
"scanner."},
{(char *)nullptr, vpParseArgv::ARGV_END, (char *)nullptr, (char *)nullptr, (char *)nullptr} };
// Read the command line options
if (vpParseArgv::parse(&argc, argv, argTable,
return (EXIT_FAILURE);
}
time_offset = vpTime::measureTimeSecond();
std::thread thread_camera_acq(&camera_acq_and_display_loop);
std::thread thread_laser_acq(&laser_acq_loop);
std::thread thread_laser_display(&laser_display_and_save_loop);
thread_camera_acq.join();
thread_laser_acq.join();
thread_laser_display.join();
return EXIT_SUCCESS;
}
catch (const vpException &e) {
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
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.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor blue
Definition: vpColor.h:217
static const vpColor yellow
Definition: vpColor.h:219
static const vpColor green
Definition: vpColor.h:214
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
Class that defines generic functionalities for display.
Definition: vpDisplay.h:173
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.
Definition: vpException.h:59
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:287
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:304
void set_i(double ii)
Definition: vpImagePoint.h:293
void quarterSizeImage(vpImage< Type > &res) const
Definition: vpImage.h:1726
unsigned int getWidth() const
Definition: vpImage.h:249
unsigned int getHeight() const
Definition: vpImage.h:184
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:818
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:967
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Definition: vpLaserScan.h:62
double getStartTimestamp()
Definition: vpLaserScan.h:111
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:90
double getEndTimestamp()
Definition: vpLaserScan.h:113
void setIpAddress(std::string ip_address)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
@ ARGV_NO_DEFAULTS
No default options like -help.
Definition: vpParseArgv.h:168
@ ARGV_NO_LEFTOVERS
Print an error message if an option is not in the argument list.
Definition: vpParseArgv.h:169
@ ARGV_INT
Argument is associated to an int.
Definition: vpParseArgv.h:152
@ ARGV_END
End of the argument list.
Definition: vpParseArgv.h:161
@ ARGV_HELP
Argument is for help displaying.
Definition: vpParseArgv.h:160
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:73
double getRadialDist() const
Definition: vpScanPoint.h:113
double getHAngle() const
Definition: vpScanPoint.h:121
Driver for the Sick LD-MRS laser scanner.
Definition: vpSickLDMRS.h:103
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()