rplidar_ros
rplidar_ros copied to clipboard
robopeak
Better and faster low level code.
I use the RPLIDAR A2 on a PIC32 microcontroller, it's a web robot here : https://www.serveurperso.com/?page=robot
I find the original driver slow and hard to read. https://github.com/robopeak/rplidar_ros/blob/master/sdk/src/rplidar_driver.cpp
This is my legacy protocol (RPLIDAR A1) non blocking code :
void readLidar() {
uint8_t current;
static uint8_t n = 0;
static uint8_t quality;
static bool start;
static uint16_t angleq6;
static uint16_t distanceq2;
static uint16_t i = 0;
float angle;
float distance;
float lidarx;
float lidary;
while(RXLIDAR.available()) {
current = RXLIDAR.read();
switch(n) {
case 0:
if((current & 1) != (current >> 1 & 1)) {
quality = current >> 2;
start = current & 1;
n = 1;
}
break;
case 1:
if(current & 1) {
angleq6 = current >> 1;
n = 2;
} else
n = 0;
break;
case 2:
angleq6 |= current << 7;
n = 3;
break;
case 3:
distanceq2 = current;
n = 4;
break;
case 4:
distanceq2 |= current << 8;
if(start) {
if(i && i < NBMESURESMAX) {
lidarReady = true;
for(uint8_t j = i; j < NBMESURESMAX; j++) {
lidarsx[j] = 0;
lidarsy[j] = 0;
lidarsCartex[j] = 0;
lidarsCartey[j] = 0;
}
}
i = 0;
} else if(i == NBMESURESMAX) {
lidarReady = false;
n = 0;
break;
}
if(distanceq2) { // Good sample
angle = float(angleq6) / 64.0 * PI / 180.0;
distance = float(distanceq2) / 4.0;
lidarx = POSITIONLIDARX + distance * sin(angle);
lidary = POSITIONLIDARY + distance * -cos(angle);
if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Inside the robot
lidarsx[i] = int(lidarx);
lidarsy[i] = int(lidary);
i++;
}
}
n = 0;
}
}
}
Now I must make the 4K (cabin) protocol decoder. This is my non terminated code :
#define RPLIDARRESPONSESIZE 84
#define RPLIDARNBSAMPLES 32
bool readLidar() {
uint8_t current;
static uint8_t n = 0;
static uint8_t checksum;
static uint8_t sum = 0;
static uint16_t oldStartAngle;
static uint16_t startAngle = 0;
static bool start;
static uint16_t diffAngle;
static uint8_t m = 0;
static uint8_t s = 0;
static uint8_t angles[RPLIDARNBSAMPLES];
static uint16_t distances[RPLIDARNBSAMPLES];
while(RXLIDAR.available()) {
current = RXLIDAR.read();
switch(n) {
case 0:
if(current >> 4 == 0xA) {
checksum = current & 0xF;
n = 1;
}
break;
case 1:
if(current >> 4 == 0x5) {
checksum |= current << 4;
n = 2;
} else
n = 0;
break;
case 2:
sum ^= current;
oldStartAngle = startAngle;
startAngle = current;
n++;
break;
case 3:
sum ^= current;
startAngle |= (current & 0x7F) << 8;
start = current >> 7;
if(start)
TXDEBUG.println('S');
if(oldStartAngle > startAngle)
diffAngle = (360 << 6) + startAngle - oldStartAngle;
else
diffAngle = startAngle - oldStartAngle;
for(uint8_t i = 0; i < RPLIDARNBSAMPLES; i++) {
//TXDEBUG.println(float(oldStartAngle * 32 + diffAngle * i - angles[i] * 64) / 1024.0);
}
n++;
break;
default:
sum ^= current;
switch(m) {
case 0:
angles[s] = (current & 3) << 4;
distances[s] = current >> 2;
m++;
break;
case 1:
distances[s] |= current << 6;
m++;
break;
case 2:
angles[s + 1] = (current & 3) << 4;
distances[s + 1] = current >> 2;
m++;
break;
case 3:
distances[s + 1] |= current << 6;
m++;
break;
case 4:
angles[s] |= current & 0xF;
angles[s + 1] |= current >> 4;
s += 2;
m = 0;
break;
}
n++;
if(n == RPLIDARRESPONSESIZE + 1) {
if(sum != checksum) {
TXDEBUG.print(sum);
TXDEBUG.print(" != ");
TXDEBUG.println(checksum);
}
n = 0;
sum = 0;
m = 0;
s = 0;
}
break;
}
}
}
4K mode is hard to decode for realtime on a microcontroller. I need current and future (datasheet), I use past and current (bug)
#define RPLIDARNBSAMPLES 32
bool readLidar() {
uint8_t current;
static uint8_t n = 0;
static uint8_t checksum;
static uint8_t sum = 0;
static uint16_t oldStartAngleq6;
static uint16_t startAngleq6 = 0;
static bool start;
static uint16_t diffAngleq6;
static uint8_t m = 0;
static uint8_t s = 0;
static int8_t derivAnglesq5[RPLIDARNBSAMPLES];
static uint16_t distances[RPLIDARNBSAMPLES];
float distance;
float angle;
static uint16_t j = 0;
float lidarx;
float lidary;
while(RXLIDAR.available()) {
current = RXLIDAR.read();
switch(n) {
case 0:
if(current >> 4 == 0xA) {
checksum = current & 0xF;
n = 1;
}
break;
case 1:
if(current >> 4 == 0x5) {
checksum |= current << 4;
n = 2;
} else
n = 0;
break;
case 2:
sum ^= current;
oldStartAngleq6 = startAngleq6;
startAngleq6 = current;
n++;
break;
case 3:
sum ^= current;
startAngleq6 |= (current & 0x7F) << 8;
start = current >> 7;
if(start)
TXDEBUG.println("start");
if(oldStartAngleq6 > startAngleq6)
diffAngleq6 = (360 << 6) + startAngleq6 - oldStartAngleq6;
else
diffAngleq6 = startAngleq6 - oldStartAngleq6;
for(uint8_t i = 0; i < RPLIDARNBSAMPLES; i++) {
distance = distances[i];
if(distance) { // Si la lecture est valide
angle = (float(oldStartAngleq6) + float(diffAngleq6) * float(i) / float(RPLIDARNBSAMPLES) - float(derivAnglesq5[i] << 1)) / 64.0 * PI / 180.0;;
lidarx = POSITIONLIDARX + distance * sin(angle);
lidary = POSITIONLIDARY + distance * -cos(angle);
if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Si la lecture est hors du robot
lidarsx[j] = int(lidarx);
lidarsy[j] = int(lidary);
j++;
if(j == NBMESURESMAX)
j = 0;
}
}
}
n++;
break;
default: // Cabin
sum ^= current;
n++;
switch(m) {
case 0:
derivAnglesq5[s] = (current & 3) << 6;
distances[s] = current >> 2;
m++;
break;
case 1:
distances[s] |= current << 6;
m++;
break;
case 2:
derivAnglesq5[s + 1] = (current & 3) << 6;
distances[s + 1] = current >> 2;
m++;
break;
case 3:
distances[s + 1] |= current << 6;
m++;
break;
case 4:
derivAnglesq5[s] |= (current & 0x0F) << 2;
derivAnglesq5[s + 1] |= (current & 0xF0) >> 2;
m = 0;
s += 2;
if(s == RPLIDARNBSAMPLES) {
if(sum != checksum) {
TXDEBUG.print(sum);
TXDEBUG.print(" != ");
TXDEBUG.println(checksum);
}
n = 0;
s = 0;
sum = 0;
}
break;
}
break;
}
}
}
can you give me a detail info about that 'I find the original driver slow and hard to read' ; more info can get form slamtec
look at https://github.com/robopeak/rplidar_ros/blob/master/sdk/src/rplidar_driver.cpp
what is the frequent about your PIC32 microcontroller? I am sorry for CR your code. you can contact with [email protected], if you have any question in analysising rplidar Protocol.
I run the first code I posted (2K legacy mode) on a 80MHz PIC32 + a lot of floating point and integer arithmetic, (a complete robot with SLAM-like) : https://www.serveurperso.com/?page=robot
Just want to upgrade to 4K but the protocol need to know point in the future to do math, it need a buffer, this kill my realtime optimization
I already contacted support but they can't hack the firmware to get 4K samples with a realtime protocol (like the 2K mode) @ faster baudrate than 115200
This issue is just to share my optimized 2K driver, it can work on a 16MHz 8bits AVR Arduino !
This issue is just to share my optimized 2K driver, it can work on a 16MHz 8bits AVR Arduino !
As-tu finalement réussi à développer un driver 4K ? Merci 👍
Oui, il supporte aussi le LD Lidar qui est meilleur en / performances / prix / taille / bruit / au soleil que le RP.
https://github.com/vigibot/vigiclient/blob/master/opencv/lidar/lidars.cpp https://github.com/vigibot/vigiclient/blob/master/opencv/lidar/lidars.hpp
C'est le driver "officiel" pour le SLAM open-source de l'asso Vigibot j'ai mis deux robots en ligne ici https://www.vigibot.com/ Le robot "Lidar" est équipé d'un RP Lidar avec le firmware 8K qui fonctionne en 4K Et le robot "Slam" est équipé d'un LD Lidar en 4.5K