Go to the documentation of this file.
19 #if MRPT_HAS_ROBOPEAK_LIDAR
21 using namespace rp::standalone::rplidar;
22 #define RPLIDAR_DRV static_cast<RPlidarDriver*>(m_rplidar_drv)
34 CRoboPeakLidar::CRoboPeakLidar() : m_com_port(
"") {
m_sensorLabel =
"RPLidar"; }
46 #if MRPT_HAS_ROBOPEAK_LIDAR
47 RPlidarDriver::DisposeDriver(RPLIDAR_DRV);
56 bool& outThereIsObservation,
59 #if MRPT_HAS_ROBOPEAK_LIDAR
60 outThereIsObservation =
false;
61 hardwareError =
false;
70 rplidar_response_measurement_node_t nodes[360 * 2];
71 size_t count =
sizeof(nodes) /
sizeof(nodes[0]);
75 u_result op_result = RPLIDAR_DRV->grabScanData(nodes, count);
81 if (op_result == RESULT_OK)
83 op_result = RPLIDAR_DRV->ascendScanData(nodes, count);
84 if (op_result == RESULT_OK)
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];
92 angle_compensate_nodes, 0,
93 angle_compensate_nodes_count *
94 sizeof(rplidar_response_measurement_node_t));
97 angle_compensate_nodes_count, 0,
false);
99 for (
size_t i = 0; i < count; i++)
101 if (nodes[i].distance_q2 != 0)
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++)
110 angle_compensate_nodes
111 [angle_value - angle_compensate_offset + j] =
117 for (
size_t i = 0; i < angle_compensate_nodes_count; i++)
119 const float read_value =
120 (float)angle_compensate_nodes[i].distance_q2 / 4.0f / 1000;
125 else if (op_result == RESULT_OPERATION_FAIL)
132 outObservation.
timestamp = tim_scan_start;
146 outThereIsObservation =
true;
150 if (op_result == RESULT_OPERATION_TIMEOUT ||
151 op_result == RESULT_OPERATION_FAIL)
166 const std::string& iniSection)
169 configSource.
read_float(iniSection,
"pose_x", 0),
170 configSource.
read_float(iniSection,
"pose_y", 0),
171 configSource.
read_float(iniSection,
"pose_z", 0),
193 #if MRPT_HAS_ROBOPEAK_LIDAR
195 if (ret && RPLIDAR_DRV) RPLIDAR_DRV->startMotor();
207 #if MRPT_HAS_ROBOPEAK_LIDAR
211 RPLIDAR_DRV->stopMotor();
222 #if MRPT_HAS_ROBOPEAK_LIDAR
223 if (!RPLIDAR_DRV)
return false;
225 rplidar_response_device_health_t healthinfo;
227 u_result op_result = RPLIDAR_DRV->getHealth(healthinfo);
228 if (IS_OK(op_result))
231 "[CRoboPeakLidar] RPLidar health status : %d\n", healthinfo.status);
232 if (healthinfo.status != RPLIDAR_STATUS_OK)
236 "[CRoboPeakLidar] Error, rplidar internal error detected. "
237 "Please reboot the device to retry.\n");
245 "[CRoboPeakLidar] Error, cannot retrieve rplidar health code: %x\n",
262 #if MRPT_HAS_ROBOPEAK_LIDAR
263 if (RPLIDAR_DRV)
return true;
267 RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
283 if (IS_FAIL(RPLIDAR_DRV->connect(
288 "[CRoboPeakLidar] Error, cannot bind to the specified serial port "
294 rplidar_response_device_info_t devinfo;
295 if (IS_FAIL(RPLIDAR_DRV->getDeviceInfo(devinfo)))
303 "[CRoboPeakLidar] Connection established:\n"
304 "Firmware version: %u\n"
305 "Hardware version: %u\n"
308 (
unsigned int)devinfo.firmware_version,
309 (
unsigned int)devinfo.hardware_version,
310 (
unsigned int)devinfo.model);
312 for (
unsigned char i : devinfo.serialnum) printf(
"%02X", i);
320 u_result res = RPLIDAR_DRV->startScan();
324 stderr,
"[CRoboPeakLidar] Error starting scanning mode: %x\n", res);
341 throw std::runtime_error(
342 "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
344 throw std::runtime_error(
345 "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
bool turnOff() override
See base class docs.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void setScanRange(const size_t i, const float val)
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
~CRoboPeakLidar() override
Destructor: turns the laser off.
bool turnOn() override
See base class docs.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
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...
bool getDeviceHealth() const
Returns true if the device is connected & operative.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
bool checkCOMMs()
Returns true if communication has been established with the device.
#define THROW_EXCEPTION(msg)
void setScanRangeValidity(const size_t i, const bool val)
This namespace contains representation of robot actions and observations.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void initialize() override
Attempts to connect and turns the laser on.
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 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 ...
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
This class allows loading and storing values and vectors of different types from a configuration text...
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).
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
std::string m_sensorLabel
See CGenericSensor.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
constexpr double DEG2RAD(const double x)
Degrees to radians
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Interfaces a Robo Peak LIDAR laser scanner.
poses::CPose3D m_sensorPose
The sensor 6D pose:
void disconnect()
Closes the comms with the laser.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
The namespace for 3D scene representation and rendering.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
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,...
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |