MRPT  2.0.3
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 }
os.h
mrpt::hwdrivers::CRoboPeakLidar::turnOff
bool turnOff() override
See base class docs.
Definition: CRoboPeakLidar.cpp:205
CClientTCPSocket.h
mrpt::obs::CObservation::sensorLabel
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
mrpt::obs::CObservation2DRangeScan::setScanRange
void setScanRange(const size_t i, const float val)
Definition: CObservation2DRangeScan.cpp:507
mrpt::obs::CObservation2DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation2DRangeScan.h:119
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:116
mrpt::hwdrivers::CRoboPeakLidar::~CRoboPeakLidar
~CRoboPeakLidar() override
Destructor: turns the laser off.
Definition: CRoboPeakLidar.cpp:38
mrpt::hwdrivers::CRoboPeakLidar::turnOn
bool turnOn() override
See base class docs.
Definition: CRoboPeakLidar.cpp:191
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::poses::CPose3D::setFromValues
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
CSerialPort.h
mrpt::hwdrivers::CRoboPeakLidar::m_com_port
std::string m_com_port
Definition: CRoboPeakLidar.h:90
mrpt::hwdrivers::CRoboPeakLidar::getDeviceHealth
bool getDeviceHealth() const
Returns true if the device is connected & operative.
Definition: CRoboPeakLidar.cpp:220
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAngles
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
Definition: C2DRangeFinderAbstract.cpp:205
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CRoboPeakLidar::checkCOMMs
bool checkCOMMs()
Returns true if communication has been established with the device.
Definition: CRoboPeakLidar.cpp:259
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::obs::CObservation2DRangeScan::setScanRangeValidity
void setScanRangeValidity(const size_t i, const bool val)
Definition: CObservation2DRangeScan.cpp:534
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation::timestamp
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::hwdrivers::CRoboPeakLidar::initialize
void initialize() override
Attempts to connect and turns the laser on.
Definition: CRoboPeakLidar.cpp:338
mrpt::hwdrivers::CGenericSensor::m_verbose
bool m_verbose
Definition: CGenericSensor.h:149
mrpt::obs::CObservation2DRangeScan::resizeScanAndAssign
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.
Definition: CObservation2DRangeScan.cpp:548
mrpt::hwdrivers::CRoboPeakLidar::setSerialPort
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 ...
Definition: CRoboPeakLidar.cpp:348
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:114
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::C2DRangeFinderAbstract::loadCommonParams
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).
Definition: C2DRangeFinderAbstract.cpp:119
mrpt::system::TTimeStamp
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
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:141
mrpt::obs::CObservation2DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation2DRangeScan.h:125
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:314
mrpt::hwdrivers::C2DRangeFinderAbstract::processPreview
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
Definition: C2DRangeFinderAbstract.cpp:211
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
mrpt::hwdrivers::CRoboPeakLidar
Interfaces a Robo Peak LIDAR laser scanner.
Definition: CRoboPeakLidar.h:50
CRoboPeakLidar.h
mrpt::hwdrivers::CRoboPeakLidar::m_sensorPose
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CRoboPeakLidar.h:89
mrpt::hwdrivers::CRoboPeakLidar::m_rplidar_drv
void * m_rplidar_drv
Definition: CRoboPeakLidar.h:92
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::hwdrivers::CRoboPeakLidar::disconnect
void disconnect()
Closes the comms with the laser.
Definition: CRoboPeakLidar.cpp:44
mrpt::hwdrivers::CRoboPeakLidar::m_com_port_baudrate
int m_com_port_baudrate
Definition: CRoboPeakLidar.h:91
M_PIf
#define M_PIf
Definition: common.h:61
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::hwdrivers::CRoboPeakLidar::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CRoboPeakLidar.cpp:164
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAreas
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
Definition: C2DRangeFinderAbstract.cpp:196
mrpt::system
Definition: backtrace.h:14
hwdrivers-precomp.h
mrpt::hwdrivers::CRoboPeakLidar::doProcessSimple
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,...
Definition: CRoboPeakLidar.cpp:55



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020