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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020