Main MRPT website > C++ reference for MRPT 1.9.9
CRoboPeakLidar.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/comms/CSerialPort.h>
15 #include <mrpt/system/os.h>
16 
18 
19 #if MRPT_HAS_ROBOPEAK_LIDAR
20 #include "rplidar.h" // RPLidar API
21 using namespace rp::standalone::rplidar;
22 #define RPLIDAR_DRV static_cast<RPlidarDriver*>(m_rplidar_drv)
23 #endif
24 
25 using namespace mrpt::obs;
26 using namespace mrpt::hwdrivers;
27 using namespace mrpt::system;
28 using namespace mrpt::opengl;
29 using namespace std;
30 
31 /*-------------------------------------------------------------
32  Constructor
33 -------------------------------------------------------------*/
34 CRoboPeakLidar::CRoboPeakLidar()
35  : m_com_port(""), m_com_port_baudrate(115200), m_rplidar_drv(nullptr)
36 {
37  m_sensorLabel = "RPLidar";
38 }
39 
40 /*-------------------------------------------------------------
41  ~CRoboPeakLidar
42 -------------------------------------------------------------*/
44 {
45  turnOff();
46  disconnect();
47 }
48 
50 {
51 #if MRPT_HAS_ROBOPEAK_LIDAR
52  RPlidarDriver::DisposeDriver(RPLIDAR_DRV);
53  m_rplidar_drv = nullptr;
54 #endif
55 }
56 
57 /*-------------------------------------------------------------
58  doProcessSimple
59 -------------------------------------------------------------*/
61  bool& outThereIsObservation,
62  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
63 {
64 #if MRPT_HAS_ROBOPEAK_LIDAR
65  outThereIsObservation = false;
66  hardwareError = false;
67 
68  // Bound?
69  if (!checkCOMMs())
70  {
71  hardwareError = true;
72  return;
73  }
74 
75  rplidar_response_measurement_node_t nodes[360 * 2];
76  size_t count = sizeof(nodes) / sizeof(nodes[0]);
77 
78  // Scan:
79  const mrpt::system::TTimeStamp tim_scan_start = mrpt::system::now();
80  u_result op_result = RPLIDAR_DRV->grabScanData(nodes, count);
81  // const mrpt::system::TTimeStamp tim_scan_end = mrpt::system::now();
82  // const double scan_duration =
83  // mrpt::system::timeDifference(tim_scan_start,tim_scan_end);
84 
85  // Fill in scan data:
86  if (op_result == RESULT_OK)
87  {
88  op_result = RPLIDAR_DRV->ascendScanData(nodes, count);
89  if (op_result == RESULT_OK)
90  {
91  const size_t angle_compensate_nodes_count = 360;
92  const size_t angle_compensate_multiple = 1;
93  int angle_compensate_offset = 0;
94  rplidar_response_measurement_node_t
95  angle_compensate_nodes[angle_compensate_nodes_count];
96  memset(
97  angle_compensate_nodes, 0,
98  angle_compensate_nodes_count *
99  sizeof(rplidar_response_measurement_node_t));
100 
101  outObservation.resizeScanAndAssign(
102  angle_compensate_nodes_count, 0, false);
103 
104  for (size_t i = 0; i < count; i++)
105  {
106  if (nodes[i].distance_q2 != 0)
107  {
108  float angle =
109  (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f);
110  int angle_value = (int)(angle * angle_compensate_multiple);
111  if ((angle_value - angle_compensate_offset) < 0)
112  angle_compensate_offset = angle_value;
113  for (size_t j = 0; j < angle_compensate_multiple; j++)
114  {
115  angle_compensate_nodes[angle_value -
116  angle_compensate_offset + j] =
117  nodes[i];
118  }
119  }
120  }
121 
122  for (size_t i = 0; i < angle_compensate_nodes_count; i++)
123  {
124  const float read_value =
125  (float)angle_compensate_nodes[i].distance_q2 / 4.0f / 1000;
126  outObservation.setScanRange(i, read_value);
127  outObservation.setScanRangeValidity(i, (read_value > 0));
128  }
129  }
130  else if (op_result == RESULT_OPERATION_FAIL)
131  {
132  // All the data is invalid, just publish them
133  outObservation.resizeScanAndAssign(count, 0, false);
134  }
135 
136  // Fill common parts of the obs:
137  outObservation.timestamp = tim_scan_start;
138  outObservation.rightToLeft = false;
139  outObservation.aperture = 2 * M_PIf;
140  outObservation.maxRange = 6.0;
141  outObservation.stdError = 0.010f;
142  outObservation.sensorPose = m_sensorPose;
143  outObservation.sensorLabel = m_sensorLabel;
144 
145  // Do filter:
148  // Do show preview:
150 
151  outThereIsObservation = true;
152  }
153  else
154  {
155  if (op_result == RESULT_OPERATION_TIMEOUT ||
156  op_result == RESULT_OPERATION_FAIL)
157  {
158  // Error? Retry connection
159  this->disconnect();
160  }
161  }
162 
163 #endif
164 }
165 
166 /*-------------------------------------------------------------
167  loadConfig_sensorSpecific
168 -------------------------------------------------------------*/
170  const mrpt::config::CConfigFileBase& configSource,
171  const std::string& iniSection)
172 {
174  configSource.read_float(iniSection, "pose_x", 0),
175  configSource.read_float(iniSection, "pose_y", 0),
176  configSource.read_float(iniSection, "pose_z", 0),
177  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
178  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
179  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
180 
181 #ifdef _WIN32
182  m_com_port =
183  configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true);
184 #else
185  m_com_port =
186  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true);
187 #endif
188 
189  // Parent options:
190  this->loadCommonParams(configSource, iniSection);
191 }
192 
193 /*-------------------------------------------------------------
194  turnOn
195 -------------------------------------------------------------*/
197 {
198 #if MRPT_HAS_ROBOPEAK_LIDAR
199  bool ret = checkCOMMs();
200  if (ret && RPLIDAR_DRV) RPLIDAR_DRV->startMotor();
201  return ret;
202 #else
203  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
204 #endif
205 }
206 
207 /*-------------------------------------------------------------
208  turnOff
209 -------------------------------------------------------------*/
211 {
212 #if MRPT_HAS_ROBOPEAK_LIDAR
213  if (RPLIDAR_DRV)
214  {
215  RPLIDAR_DRV->stop();
216  RPLIDAR_DRV->stopMotor();
217  }
218  return true;
219 #else
220  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
221 #endif
222 }
223 
224 /** Returns true if the device is connected & operative */
226 {
227 #if MRPT_HAS_ROBOPEAK_LIDAR
228  if (!RPLIDAR_DRV) return false;
229 
230  rplidar_response_device_health_t healthinfo;
231 
232  u_result op_result = RPLIDAR_DRV->getHealth(healthinfo);
233  if (IS_OK(op_result))
234  {
235  printf(
236  "[CRoboPeakLidar] RPLidar health status : %d\n", healthinfo.status);
237  if (healthinfo.status != RPLIDAR_STATUS_OK)
238  {
239  fprintf(
240  stderr,
241  "[CRoboPeakLidar] Error, rplidar internal error detected. "
242  "Please reboot the device to retry.\n");
243  return false;
244  }
245  }
246  else
247  {
248  fprintf(
249  stderr,
250  "[CRoboPeakLidar] Error, cannot retrieve rplidar health code: %x\n",
251  op_result);
252  return false;
253  }
254 
255  return true;
256 #else
257  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
258 #endif
259 }
260 
261 /*-------------------------------------------------------------
262  checkCOMMs
263 -------------------------------------------------------------*/
265 {
266  MRPT_START
267 #if MRPT_HAS_ROBOPEAK_LIDAR
268  if (RPLIDAR_DRV) return true;
269 
270  // create the driver instance
271  m_rplidar_drv =
272  RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
273  ASSERTMSG_(m_rplidar_drv, "Create Driver failed.");
274 
275  // Is it COMX, X>4? -> "\\.\COMX"
276  if (m_com_port.size() >= 3)
277  {
278  if (tolower(m_com_port[0]) == 'c' && tolower(m_com_port[1]) == 'o' &&
279  tolower(m_com_port[2]) == 'm')
280  {
281  // Need to add "\\.\"?
282  if (m_com_port.size() > 4 || m_com_port[3] > '4')
283  m_com_port = std::string("\\\\.\\") + m_com_port;
284  }
285  }
286 
287  // make connection...
288  if (IS_FAIL(
289  RPLIDAR_DRV->connect(
290  m_com_port.c_str(), (_u32)m_com_port_baudrate)))
291  {
292  fprintf(
293  stderr,
294  "[CRoboPeakLidar] Error, cannot bind to the specified serial port "
295  "%s\n",
296  m_com_port.c_str());
297  return false;
298  }
299 
300  rplidar_response_device_info_t devinfo;
301  if (IS_FAIL(RPLIDAR_DRV->getDeviceInfo(devinfo)))
302  {
303  return false;
304  }
305 
306  if (m_verbose)
307  {
308  printf(
309  "[CRoboPeakLidar] Connection established:\n"
310  "Firmware version: %u\n"
311  "Hardware version: %u\n"
312  "Model: %u\n"
313  "Serial: ",
314  (unsigned int)devinfo.firmware_version,
315  (unsigned int)devinfo.hardware_version,
316  (unsigned int)devinfo.model);
317 
318  for (int i = 0; i < 16; i++) printf("%02X", devinfo.serialnum[i]);
319  printf("\n");
320  }
321 
322  // check health:
323  if (!getDeviceHealth()) return false;
324 
325  // start scan...
326  u_result res = RPLIDAR_DRV->startScan();
327  if (IS_FAIL(res))
328  {
329  fprintf(
330  stderr, "[CRoboPeakLidar] Error starting scanning mode: %x\n", res);
331  return false;
332  }
333 
334  return true;
335 #else
336  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
337 #endif
338  MRPT_END
339 }
340 
341 /*-------------------------------------------------------------
342  initialize
343 -------------------------------------------------------------*/
345 {
346  if (!checkCOMMs())
347  throw std::runtime_error(
348  "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
349  if (!turnOn())
350  throw std::runtime_error(
351  "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
352 }
353 
355 {
356  if (m_rplidar_drv)
357  THROW_EXCEPTION("Can't change serial port while connected!");
358 
359  m_com_port = port_name;
360 }
GLuint GLuint GLsizei count
Definition: glext.h:3528
virtual void initialize()
Attempts to connect and turns the laser on.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
#define IS_OK(x)
Definition: rptypes.h:108
#define MRPT_START
Definition: exceptions.h:262
void setSerialPort(const std::string &port_name)
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
Definition: rplidar_cmd.h:113
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
_u16 angle_q6_checkbit
Definition: rplidar_cmd.h:20
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
void disconnect()
Closes the comms with the laser.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
virtual void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)
Specific laser scanner "software drivers" must process here new data from the I/O stream...
bool getDeviceHealth() const
Returns true if the device is connected & operative.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
#define RESULT_OK
Definition: rptypes.h:97
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
float maxRange
The maximum range allowed by the device, in meters (e.g.
#define IS_FAIL(x)
Definition: rptypes.h:109
This class allows loading and storing values and vectors of different types from a configuration text...
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
Interfaces a Robo Peak LIDAR laser scanner.
void resizeScanAndAssign(const size_t len, const float rangeVal, const bool rangeValidity, const int32_t rangeIntensity=0)
Resizes all data vectors to allocate a given number of scan rays and assign default values...
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define RESULT_OPERATION_TIMEOUT
Definition: rptypes.h:102
bool checkCOMMs()
Returns true if communication has been established with the device.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
virtual bool turnOff()
See base class docs.
virtual bool turnOn()
See base class docs.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
_u16 distance_q2
Definition: rplidar_cmd.h:21
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
poses::CPose3D m_sensorPose
The sensor 6D pose:
#define MRPT_END
Definition: exceptions.h:266
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
uint32_t _u32
Definition: rptypes.h:66
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
#define RESULT_OPERATION_FAIL
Definition: rptypes.h:101
GLuint res
Definition: glext.h:7268
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
virtual ~CRoboPeakLidar()
Destructor: turns the laser off.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
uint32_t u_result
Definition: rptypes.h:95
void setScanRangeValidity(const size_t i, const bool val)
#define RPLIDAR_STATUS_OK
Definition: rplidar_cmd.h:106



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019