19 #if MRPT_HAS_ROBOPEAK_LIDAR 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]);
83 op_result = RPLIDAR_DRV->ascendScanData(nodes,
count);
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++)
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;
132 outObservation.
timestamp = tim_scan_start;
146 outThereIsObservation =
true;
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);
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);
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.");
GLuint GLuint GLsizei count
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 §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
#define THROW_EXCEPTION(msg)
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
void disconnect()
Closes the comms with the laser.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
bool getDeviceHealth() const
Returns true if the device is connected & operative.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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...
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...
This namespace contains representation of robot actions and observations.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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...
GLsizei const GLchar ** string
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define RESULT_OPERATION_TIMEOUT
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.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
poses::CPose3D m_sensorPose
The sensor 6D pose:
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...
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.
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.
#define RESULT_OPERATION_FAIL
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)
#define RPLIDAR_STATUS_OK