Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
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::utils;
26 using namespace mrpt::obs;
27 using namespace mrpt::hwdrivers;
28 using namespace mrpt::system;
29 using namespace mrpt::opengl;
30 using namespace std;
31 
32 
33 /*-------------------------------------------------------------
34  Constructor
35 -------------------------------------------------------------*/
36 CRoboPeakLidar::CRoboPeakLidar() :
37  m_com_port(""),
38  m_com_port_baudrate(115200),
39  m_rplidar_drv(NULL)
40 {
41  m_sensorLabel = "RPLidar";
42 }
43 
44 /*-------------------------------------------------------------
45  ~CRoboPeakLidar
46 -------------------------------------------------------------*/
48 {
49  turnOff();
50  disconnect();
51 }
52 
54 {
55 #if MRPT_HAS_ROBOPEAK_LIDAR
56  RPlidarDriver::DisposeDriver(RPLIDAR_DRV);
57  m_rplidar_drv=NULL;
58 #endif
59 }
60 
61 /*-------------------------------------------------------------
62  doProcessSimple
63 -------------------------------------------------------------*/
65  bool &outThereIsObservation,
66  mrpt::obs::CObservation2DRangeScan &outObservation,
67  bool &hardwareError )
68 {
69 #if MRPT_HAS_ROBOPEAK_LIDAR
70  outThereIsObservation = false;
71  hardwareError = false;
72 
73  // Bound?
74  if (!checkCOMMs())
75  {
76  hardwareError = true;
77  return;
78  }
79 
80  rplidar_response_measurement_node_t nodes[360*2];
81  size_t count = sizeof(nodes)/sizeof(nodes[0]);
82 
83  // Scan:
84  const mrpt::system::TTimeStamp tim_scan_start = mrpt::system::now();
85  u_result op_result = RPLIDAR_DRV->grabScanData(nodes, count);
86  //const mrpt::system::TTimeStamp tim_scan_end = mrpt::system::now();
87  //const double scan_duration = mrpt::system::timeDifference(tim_scan_start,tim_scan_end);
88 
89  // Fill in scan data:
90  if (op_result == RESULT_OK)
91  {
92  op_result = RPLIDAR_DRV->ascendScanData(nodes, count);
93  if (op_result == RESULT_OK)
94  {
95  const size_t angle_compensate_nodes_count = 360;
96  const size_t angle_compensate_multiple = 1;
97  int angle_compensate_offset = 0;
98  rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
99  memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
100 
101  outObservation.resizeScanAndAssign(angle_compensate_nodes_count, 0, false);
102 
103  for(size_t i=0 ; i < count; i++ )
104  {
105  if (nodes[i].distance_q2 != 0)
106  {
107  float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
108  int angle_value = (int)(angle * angle_compensate_multiple);
109  if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
110  for (size_t j = 0; j < angle_compensate_multiple; j++)
111  {
112  angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
113  }
114  }
115  }
116 
117  for(size_t i=0 ; i < angle_compensate_nodes_count; i++ )
118  {
119  const float read_value = (float) angle_compensate_nodes[i].distance_q2/4.0f/1000;
120  outObservation.setScanRange(i, read_value );
121  outObservation.setScanRangeValidity(i, (read_value>0) );
122  }
123  }
124  else if (op_result == RESULT_OPERATION_FAIL)
125  {
126  // All the data is invalid, just publish them
127  outObservation.resizeScanAndAssign(count, 0, false);
128  }
129 
130  // Fill common parts of the obs:
131  outObservation.timestamp = tim_scan_start;
132  outObservation.rightToLeft = false;
133  outObservation.aperture = 2*M_PIf;
134  outObservation.maxRange = 6.0;
135  outObservation.stdError = 0.010f;
136  outObservation.sensorPose = m_sensorPose;
137  outObservation.sensorLabel = m_sensorLabel;
138 
139  // Do filter:
142  // Do show preview:
144 
145  outThereIsObservation = true;
146  }
147  else
148  {
149  if (op_result==RESULT_OPERATION_TIMEOUT || op_result==RESULT_OPERATION_FAIL)
150  {
151  // Error? Retry connection
152  this->disconnect();
153  }
154  }
155 
156 
157 #endif
158 }
159 
160 /*-------------------------------------------------------------
161  loadConfig_sensorSpecific
162 -------------------------------------------------------------*/
164  const mrpt::utils::CConfigFileBase &configSource,
165  const std::string &iniSection )
166 {
168  configSource.read_float(iniSection,"pose_x",0),
169  configSource.read_float(iniSection,"pose_y",0),
170  configSource.read_float(iniSection,"pose_z",0),
171  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
172  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
173  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
174  );
175 
176 #ifdef MRPT_OS_WINDOWS
177  m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true );
178 #else
179  m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true );
180 #endif
181 
182  // Parent options:
183  this->loadCommonParams(configSource,iniSection);
184 }
185 
186 /*-------------------------------------------------------------
187  turnOn
188 -------------------------------------------------------------*/
190 {
191 #if MRPT_HAS_ROBOPEAK_LIDAR
192  bool ret = checkCOMMs();
193  if (ret && RPLIDAR_DRV)
194  RPLIDAR_DRV->startMotor();
195  return ret;
196 #else
197  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!")
198 #endif
199 }
200 
201 /*-------------------------------------------------------------
202  turnOff
203 -------------------------------------------------------------*/
205 {
206 #if MRPT_HAS_ROBOPEAK_LIDAR
207  if (RPLIDAR_DRV) {
208  RPLIDAR_DRV->stop();
209  RPLIDAR_DRV->stopMotor();
210  }
211  return true;
212 #else
213  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!")
214 #endif
215 }
216 
217 /** Returns true if the device is connected & operative */
219 {
220 #if MRPT_HAS_ROBOPEAK_LIDAR
221  if (!RPLIDAR_DRV) return false;
222 
223  rplidar_response_device_health_t healthinfo;
224 
225  u_result op_result = RPLIDAR_DRV->getHealth(healthinfo);
226  if (IS_OK(op_result))
227  {
228  printf("[CRoboPeakLidar] RPLidar health status : %d\n", healthinfo.status);
229  if (healthinfo.status != RPLIDAR_STATUS_OK)
230  {
231  fprintf(stderr, "[CRoboPeakLidar] Error, rplidar internal error detected. Please reboot the device to retry.\n");
232  return false;
233  }
234  }
235  else
236  {
237  fprintf(stderr, "[CRoboPeakLidar] Error, cannot retrieve rplidar health code: %x\n", op_result);
238  return false;
239  }
240 
241  return true;
242 #else
243  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!")
244  #endif
245 }
246 
247 
248 
249 /*-------------------------------------------------------------
250  checkCOMMs
251 -------------------------------------------------------------*/
253 {
254  MRPT_START
255  #if MRPT_HAS_ROBOPEAK_LIDAR
256  if (RPLIDAR_DRV)
257  return true;
258 
259  // create the driver instance
260  m_rplidar_drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
261  ASSERTMSG_(m_rplidar_drv, "Create Driver failed.")
262 
263  // Is it COMX, X>4? -> "\\.\COMX"
264  if (m_com_port.size()>=3)
265  {
266  if ( tolower( m_com_port[0]) =='c' && tolower( m_com_port[1]) =='o' && tolower( m_com_port[2]) =='m' )
267  {
268  // Need to add "\\.\"?
269  if (m_com_port.size()>4 || m_com_port[3]>'4')
270  m_com_port = std::string("\\\\.\\") + m_com_port;
271  }
272  }
273 
274  // make connection...
275  if (IS_FAIL(RPLIDAR_DRV->connect( m_com_port.c_str(), (_u32)m_com_port_baudrate)))
276  {
277  fprintf(stderr, "[CRoboPeakLidar] Error, cannot bind to the specified serial port %s\n", m_com_port.c_str() );
278  return false;
279  }
280 
281  rplidar_response_device_info_t devinfo;
282  if (IS_FAIL(RPLIDAR_DRV->getDeviceInfo(devinfo) ))
283  {
284  return false;
285  }
286 
287  if (m_verbose)
288  {
289  printf("[CRoboPeakLidar] Connection established:\n"
290  "Firmware version: %u\n"
291  "Hardware version: %u\n"
292  "Model: %u\n"
293  "Serial: ",
294  (unsigned int)devinfo.firmware_version,
295  (unsigned int)devinfo.hardware_version,
296  (unsigned int)devinfo.model);
297 
298  for (int i=0;i<16;i++)
299  printf("%02X",devinfo.serialnum[i]);
300  printf("\n");
301  }
302 
303  // check health:
304  if (!getDeviceHealth())
305  return false;
306 
307  // start scan...
308  u_result res = RPLIDAR_DRV->startScan();
309  if (IS_FAIL( res ))
310  {
311  fprintf(stderr, "[CRoboPeakLidar] Error starting scanning mode: %x\n", res);
312  return false;
313  }
314 
315  return true;
316 #else
317  THROW_EXCEPTION("MRPT has been compiled without RPLidar support!")
318  #endif
319  MRPT_END
320 }
321 
322 /*-------------------------------------------------------------
323  initialize
324 -------------------------------------------------------------*/
326 {
327  if (!checkCOMMs())
328  throw std::runtime_error("[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
329  if (!turnOn())
330  throw std::runtime_error("[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
331 }
332 
333 
335 {
336  if (m_rplidar_drv)
337  THROW_EXCEPTION("Can't change serial port while connected!")
338 
339  m_com_port = port_name;
340 }
341 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
virtual void initialize()
Attempts to connect and turns the laser on. Raises an exception on error.
#define IS_OK(x)
Definition: rptypes.h:113
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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 ...
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
Definition: rplidar_cmd.h:113
_u16 angle_q6_checkbit
Definition: rplidar_cmd.h:22
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
void disconnect()
Closes the comms with the laser. Shouldn't have to be directly needed by the user.
#define THROW_EXCEPTION(msg)
bool getDeviceHealth() const
Returns true if the device is connected & operative.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
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...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
#define RESULT_OK
Definition: rptypes.h:102
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
This class allows loading and storing values and vectors of different types from a configuration text...
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define IS_FAIL(x)
Definition: rptypes.h:114
#define M_PIf
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
#define MRPT_END
Interfaces a Robo Peak LIDAR laser scanner.
#define DEG2RAD
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
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...
void loadCommonParams(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define RESULT_OPERATION_TIMEOUT
Definition: rptypes.h:107
bool checkCOMMs()
Returns true if communication has been established with the device.
GLuint res
Definition: glew.h:7143
virtual bool turnOff()
See base class docs.
virtual bool turnOn()
See base class docs.
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
_u16 distance_q2
Definition: rplidar_cmd.h:23
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
poses::CPose3D m_sensorPose
The sensor 6D pose:
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:254
uint32_t _u32
Definition: rptypes.h:69
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
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:106
GLuint GLuint GLsizei count
Definition: glew.h:1167
GLdouble angle
Definition: glew.h:5105
#define ASSERTMSG_(f, __ERROR_MSG)
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:100
void setScanRangeValidity(const size_t i, const bool val)
#define RPLIDAR_STATUS_OK
Definition: rplidar_cmd.h:106
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018