19 #if MRPT_HAS_ROBOPEAK_LIDAR 22 #define RPLIDAR_DRV static_cast<RPlidarDriver*>(m_rplidar_drv) 34 CRoboPeakLidar::CRoboPeakLidar()
35 : m_com_port(
""), m_com_port_baudrate(115200), m_rplidar_drv(nullptr)
51 #if MRPT_HAS_ROBOPEAK_LIDAR 52 RPlidarDriver::DisposeDriver(RPLIDAR_DRV);
61 bool& outThereIsObservation,
64 #if MRPT_HAS_ROBOPEAK_LIDAR 65 outThereIsObservation =
false;
66 hardwareError =
false;
75 rplidar_response_measurement_node_t nodes[360 * 2];
76 size_t count =
sizeof(nodes) /
sizeof(nodes[0]);
88 op_result = RPLIDAR_DRV->ascendScanData(nodes,
count);
91 const size_t angle_compensate_nodes_count = 360;
92 const size_t angle_compensate_multiple = 1;
93 int angle_compensate_offset = 0;
94 rplidar_response_measurement_node_t
95 angle_compensate_nodes[angle_compensate_nodes_count];
97 angle_compensate_nodes, 0,
98 angle_compensate_nodes_count *
99 sizeof(rplidar_response_measurement_node_t));
102 angle_compensate_nodes_count, 0,
false);
104 for (
size_t i = 0; i <
count; i++)
110 int angle_value = (int)(angle * angle_compensate_multiple);
111 if ((angle_value - angle_compensate_offset) < 0)
112 angle_compensate_offset = angle_value;
113 for (
size_t j = 0; j < angle_compensate_multiple; j++)
115 angle_compensate_nodes[angle_value -
116 angle_compensate_offset + j] =
122 for (
size_t i = 0; i < angle_compensate_nodes_count; i++)
124 const float read_value =
125 (float)angle_compensate_nodes[i].
distance_q2 / 4.0f / 1000;
137 outObservation.
timestamp = tim_scan_start;
151 outThereIsObservation =
true;
174 configSource.
read_float(iniSection,
"pose_x", 0),
175 configSource.
read_float(iniSection,
"pose_y", 0),
176 configSource.
read_float(iniSection,
"pose_z", 0),
198 #if MRPT_HAS_ROBOPEAK_LIDAR 200 if (ret && RPLIDAR_DRV) RPLIDAR_DRV->startMotor();
212 #if MRPT_HAS_ROBOPEAK_LIDAR 216 RPLIDAR_DRV->stopMotor();
227 #if MRPT_HAS_ROBOPEAK_LIDAR 228 if (!RPLIDAR_DRV)
return false;
230 rplidar_response_device_health_t healthinfo;
232 u_result op_result = RPLIDAR_DRV->getHealth(healthinfo);
233 if (
IS_OK(op_result))
236 "[CRoboPeakLidar] RPLidar health status : %d\n", healthinfo.status);
241 "[CRoboPeakLidar] Error, rplidar internal error detected. " 242 "Please reboot the device to retry.\n");
250 "[CRoboPeakLidar] Error, cannot retrieve rplidar health code: %x\n",
267 #if MRPT_HAS_ROBOPEAK_LIDAR 268 if (RPLIDAR_DRV)
return true;
272 RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
289 RPLIDAR_DRV->connect(
294 "[CRoboPeakLidar] Error, cannot bind to the specified serial port " 300 rplidar_response_device_info_t devinfo;
301 if (
IS_FAIL(RPLIDAR_DRV->getDeviceInfo(devinfo)))
309 "[CRoboPeakLidar] Connection established:\n" 310 "Firmware version: %u\n" 311 "Hardware version: %u\n" 314 (
unsigned int)devinfo.firmware_version,
315 (
unsigned int)devinfo.hardware_version,
316 (
unsigned int)devinfo.model);
318 for (
int i = 0; i < 16; i++) printf(
"%02X", devinfo.serialnum[i]);
330 stderr,
"[CRoboPeakLidar] Error starting scanning mode: %x\n",
res);
347 throw std::runtime_error(
348 "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
350 throw std::runtime_error(
351 "[CRoboPeakLidar::initialize] Error initializing RPLIDAR scanner.");
GLuint GLuint GLsizei count
virtual void initialize()
Attempts to connect and turns the laser on.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
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.
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...
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 filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
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...
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.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
virtual bool turnOff()
See base class docs.
virtual bool turnOn()
See base class docs.
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)...
#define RESULT_OPERATION_FAIL
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.
void setScanRangeValidity(const size_t i, const bool val)
#define RPLIDAR_STATUS_OK