Debian Codegraph @

libs/hwdrivers/src/CRoboPeakLidar.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/comms/CClientTCPSocket.h>
13 #include <mrpt/comms/CSerialPort.h>
14 #include <mrpt/hwdrivers/CRoboPeakLidar.h>
15 #include <mrpt/system/os.h>
16
17 IMPLEMENTS_GENERIC_SENSOR(CRoboPeakLidar, mrpt::hwdrivers)
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 -------------------------------------------------------------*/
38 CRoboPeakLidar::~CRoboPeakLidar()
39 {
40 turnOff();
41 disconnect();
42 }
43
44 void CRoboPeakLidar::disconnect()
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 -------------------------------------------------------------*/
55 void CRoboPeakLidar::doProcessSimple(
56 bool& outThereIsObservation,
57 mrpt::obs::CObservation2DRangeScan& outObservation,
58 bool& hardwareError)
59 {
60 #if MRPT_HAS_ROBOPEAK_LIDAR
61 outThereIsObservation = false;
62 hardwareError = false;
63
64 // Bound?
65 if (!checkCOMMs())
66 {
67 hardwareError = true;
68 return;
69 }
70
71 rplidar_response_measurement_node_hq_t nodes[360 * 2];
72 size_t count = sizeof(nodes) / sizeof(nodes[0]);
73
74 // Scan:
75 const mrpt::system::TTimeStamp tim_scan_start = mrpt::Clock::now();
76 u_result op_result = RPLIDAR_DRV->grabScanDataHq(nodes, count);
77
78 // const mrpt::system::TTimeStamp tim_scan_end = mrpt::Clock::now();
79 // const double scan_duration =
80 // mrpt::system::timeDifference(tim_scan_start,tim_scan_end);
81
82 // Fill in scan data:
83 if (op_result == RESULT_OK)
84 {
85 op_result = RPLIDAR_DRV->ascendScanData(nodes, count);
86 if (op_result == RESULT_OK)
87 {
88 const size_t angle_compensate_nodes_count = 360;
89 const size_t angle_compensate_multiple = 1;
90 int angle_compensate_offset = 0;
91 rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];
92 memset(
93 angle_compensate_nodes, 0,
94 angle_compensate_nodes_count * sizeof(angle_compensate_nodes[0]));
95
96 outObservation.resizeScanAndAssign(angle_compensate_nodes_count, 0, false);
97
98 for (size_t i = 0; i < count; i++)
99 {
100 if (nodes[i].dist_mm_q2 != 0)
101 {
102 auto angle =
103 (float)((nodes[i].angle_z_q14 >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f);
104 int angle_value = (int)(angle * angle_compensate_multiple);
105 if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
106 for (size_t j = 0; j < angle_compensate_multiple; j++)
107 {
108 angle_compensate_nodes[angle_value - angle_compensate_offset + j] = nodes[i];
109 }
110 }
111 }
112
113 for (size_t i = 0; i < angle_compensate_nodes_count; i++)
114 {
115 const float read_value = (float)angle_compensate_nodes[i].dist_mm_q2 / 4.0f / 1000;
116 outObservation.setScanRange(i, read_value);
117 outObservation.setScanRangeValidity(i, (read_value > 0));
118 }
119 }
120 else if (op_result == RESULT_OPERATION_FAIL)
121 {
122 // All the data is invalid, just publish them
123 outObservation.resizeScanAndAssign(count, 0, false);
124 }
125
126 // Fill common parts of the obs:
127 outObservation.timestamp = tim_scan_start;
128 outObservation.rightToLeft = false;
129 outObservation.aperture = 2 * M_PIf;
130 outObservation.maxRange = 6.0;
131 outObservation.stdError = 0.010f;
132 outObservation.sensorPose = m_sensorPose;
133 outObservation.sensorLabel = m_sensorLabel;
134
135 // Do filter:
136 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
137 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
138 // Do show preview:
139 C2DRangeFinderAbstract::processPreview(outObservation);
140
141 outThereIsObservation = true;
142 }
143 else
144 {
145 if (op_result == RESULT_OPERATION_TIMEOUT || op_result == RESULT_OPERATION_FAIL)
146 {
147 // Error? Retry connection
148 this->disconnect();
149 }
150 }
151
152 #endif
153 }
154
155 /*-------------------------------------------------------------
156 loadConfig_sensorSpecific
157 -------------------------------------------------------------*/
158 void CRoboPeakLidar::loadConfig_sensorSpecific(
159 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
160 {
161 m_sensorPose.setFromValues(
162 configSource.read_float(iniSection, "pose_x", 0),
163 configSource.read_float(iniSection, "pose_y", 0),
164 configSource.read_float(iniSection, "pose_z", 0),
165 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
166 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
167 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
168
169 #ifdef _WIN32
170 m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true);
171 #else
172 m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true);
173 #endif
174
175 // Parent options:
176 this->loadCommonParams(configSource, iniSection);
177 }
178
179 /*-------------------------------------------------------------
180 turnOn
181 -------------------------------------------------------------*/
182 bool CRoboPeakLidar::turnOn()
183 {
184 #if MRPT_HAS_ROBOPEAK_LIDAR
185 bool ret = checkCOMMs();
186 if (ret && RPLIDAR_DRV) RPLIDAR_DRV->startMotor();
187 return ret;
188 #else
189 THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
190 #endif
191 }
192
193 /*-------------------------------------------------------------
194 turnOff
195 -------------------------------------------------------------*/
196 bool CRoboPeakLidar::turnOff()
197 {
198 #if MRPT_HAS_ROBOPEAK_LIDAR
199 if (RPLIDAR_DRV)
200 {
201 RPLIDAR_DRV->stop();
202 RPLIDAR_DRV->stopMotor();
203 }
204 return true;
205 #else
206 THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
207 #endif
208 }
209
210 /** Returns true if the device is connected & operative */
211 bool CRoboPeakLidar::getDeviceHealth() const
212 {
213 #if MRPT_HAS_ROBOPEAK_LIDAR
214 if (!RPLIDAR_DRV) return false;
215
216 rplidar_response_device_health_t healthinfo;
217
218 u_result op_result = RPLIDAR_DRV->getHealth(healthinfo);
219 if (IS_OK(op_result))
220 {
221 printf("[CRoboPeakLidar] RPLidar health status : %d\n", healthinfo.status);
222 if (healthinfo.status != RPLIDAR_STATUS_OK)
223 {
224 fprintf(
225 stderr,
226 "[CRoboPeakLidar] Error, rplidar internal error detected. "
227 "Please reboot the device to retry.\n");
228 return false;
229 }
230 }
231 else
232 {
233 fprintf(stderr, "[CRoboPeakLidar] Error, cannot retrieve rplidar health code: %x\n", op_result);
234 return false;
235 }
236
237 return true;
238 #else
239 THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
240 #endif
241 }
242
243 /*-------------------------------------------------------------
244 checkCOMMs
245 -------------------------------------------------------------*/
246 bool CRoboPeakLidar::checkCOMMs()
247 {
248 MRPT_START
249 #if MRPT_HAS_ROBOPEAK_LIDAR
250 if (RPLIDAR_DRV) return true;
251
252 // create the driver instance
253 m_rplidar_drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
254 ASSERTMSG_(m_rplidar_drv, "Create Driver failed.");
255
256 // Is it COMX, X>4? -> "\\.\COMX"
257 if (m_com_port.size() >= 3)
258 {
259 if (tolower(m_com_port[0]) == 'c' && tolower(m_com_port[1]) == 'o' &&
260 tolower(m_com_port[2]) == 'm')
261 {
262 // Need to add "\\.\"?
263 if (m_com_port.size() > 4 || m_com_port[3] > '4')
264 m_com_port = std::string("\\\\.\\") + m_com_port;
265 }
266 }
267
268 // make connection...
269 if (IS_FAIL(RPLIDAR_DRV->connect(m_com_port.c_str(), (_u32)m_com_port_baudrate)))
270 {
271 fprintf(
272 stderr,
273 "[CRoboPeakLidar] Error, cannot bind to the specified serial port "
274 "%s\n",
275 m_com_port.c_str());
276 return false;
277 }
278
279 rplidar_response_device_info_t devinfo;
280 if (IS_FAIL(RPLIDAR_DRV->getDeviceInfo(devinfo)))
281 {
282 return false;
283 }
284
285 if (m_verbose)
286 {
287 printf(
288 "[CRoboPeakLidar] Connection established:\n"
289 "Firmware version: %u\n"
290 "Hardware version: %u\n"
291 "Model: %u\n"
292 "Serial: ",
293 (unsigned int)devinfo.firmware_version, (unsigned int)devinfo.hardware_version,
294 (unsigned int)devinfo.model);
295
296 for (unsigned char i : devinfo.serialnum) printf("%02X", i);
297 printf("\n");
298 }
299
300 // check health:
301 if (!getDeviceHealth()) return false;
302
303 // start scan...
304 u_result res = RPLIDAR_DRV->startScan(false, true);
305 if (IS_FAIL(res))
306 {
307 fprintf(stderr, "[CRoboPeakLidar] Error starting scanning mode: %x\n", res);
308 return false;
309 }
310
311 return true;
312 #else
313 THROW_EXCEPTION("MRPT has been compiled without RPLidar support!");
314 #endif
315 MRPT_END
316 }
317
318 /*-------------------------------------------------------------
319 initialize
320 -------------------------------------------------------------*/
321 void CRoboPeakLidar::initialize()
322 {
323 if (!checkCOMMs())
324 throw std::runtime_error("[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
325 if (!turnOn())
326 throw std::runtime_error("[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
327 }
328
329 void CRoboPeakLidar::setSerialPort(const std::string& port_name)
330 {
331 if (m_rplidar_drv) THROW_EXCEPTION("Can't change serial port while connected!");
332
333 m_com_port = port_name;
334 }