YDLidar X4 scan_completed at 340 degree

[Skara77]

Hi, with ydlidar x4 and esp32 scan_completed at 340 degree. How to modify the code to get 360 degree, means full scan with scan_completed.

[kaiaai]

Hello,
the code sends scan_completed when X4 sends the scan-completed flag.
If you are running a sample sketch, change the line 'if (i % 20 == 0) {' to 'if (i % 1 == 0) {'

[Skara77]

Thanks for quick reply; I already did it before I shared the issue, So, it is not solution. Any other idea? Another comment; Do you plan to add pid motor control for ydlidar x4 library? I can not see it in LDS_YDLIDAR_X4.cpp

[kaiaai]

Are you getting angles 340-360 *after* scan_completed? Yes, I'd like to add X4 PID, but this is low priority. You are welcome to contribute.

…

On Wed, Jan 1, 2025, 10:01 AM Skara77 ***@***.***> wrote: Thanks for quick reply; I already did it before I shared the issue, So, it is not solution. Any other idea? Another comment; Do you plan to add pid motor control for ydlidar x4 library? I can not see it in LDS_YDLIDAR_X4.cpp — Reply to this email directly, view it on GitHub <#11 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AIAIQ5OZ434UZDSVL3B4ZVL2IQUODAVCNFSM6AAAAABUOPBQF6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDKNRXGA4TMMJTGE> . You are receiving this because you commented.Message ID: ***@***.***>

[Skara77]

340 to 340, in reality; -20 degree to 340 degree. But sometimes it is -18 to 346, or -19 to 342. Not fixed values. I am trying to add esp32 ros2 wifi micro-ros arduino. I can manage 340 degree issue in /scan topic infact. I already completed the code but in rviz2 scan message is vibrating. This is my main issue. I checked your github, I can not find esp32 ros2 code for ydlidar x4. For, pid, When I will have time I will try to add pid control to ydlidar x4.

[kaiaai]

Assuming you are handling scan_complete correctly when publishing /scan, is it possible your X4 is damaged? Do you have another X4 to check? X4 has a built-in PID. It keeps its speed rock steady - no vibrating/wobbling.

…

On Wed, Jan 1, 2025, 10:13 AM Skara77 ***@***.***> wrote: 340 to 340, in reality; -20 degree to 340 degree. I am trying to add esp32 ros2 wifi micro-ros arduino. I can manage this issue in fact. I already completed the code but in rviz2 scan message is vibrating. This is my main issue. When I will have time I will try to add pid control to ydlidar x4. — Reply to this email directly, view it on GitHub <#11 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AIAIQ5MCBQ2X6ZPLPFHILQL2IQV33AVCNFSM6AAAAABUOPBQF6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDKNRXGA4TSNRQGI> . You are receiving this because you commented.Message ID: ***@***.***>

[Skara77]

I am checking scan_completed with your example; ydlidar_x4_esp32.ino. I don't have other lidar to check it. But works fine when connected to pc with usb adapter. In my code, rviz2 scan image 4 - 5 degree shifting. This is my code;

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/laser_scan.h>
#include <LDS_YDLIDAR_X4.h>

#if !defined(ESP32)
#error This example is only available for ESP32 Dev module
#endif

#define FRAME_ID "laser_frame"
#define TOPIC_NAME "scan"
#define NODE_NAME "uros_node"
#define BUF_LEN 1000

#define LED_PIN 2
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

const uint8_t LIDAR_EN_PIN = 19; // ESP32 Dev Kit LiDAR enable pin
const uint8_t LIDAR_PWM_PIN = 15; // LiDAR motor speed control using PWM
const uint8_t LIDAR_TX_PIN = 17;
const uint8_t LIDAR_RX_PIN = 16;

#define LIDAR_PWM_FREQ 10000
#define LIDAR_PWM_BITS 11
#define LIDAR_PWM_CHANNEL 2 // ESP32 PWM channel for LiDAR motor speed control

HardwareSerial LidarSerial(1);
LDS_YDLIDAR_X4 lidar;

rcl_publisher_t publisher;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

sensor_msgs__msg__LaserScan laserScanMsg;
struct timespec tv = {0};
float tv_old = 0;
float tv_new = 0;

static float memory[BUF_LEN];
static float mem_intensity[BUF_LEN];
static float dist_offset = 0.15;

void error_loop(){
Serial.println(F("error_loop()"));
while(1){
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(500);
}
}

void setup() {
Serial.begin(921600);
set_microros_wifi_transports((char*)"Wifi", (char*)"Password",
(char*)"192.168.1.200", 8888);

pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);

memset((void*)&laserScanMsg, 0, sizeof(laserScanMsg));

laserScanMsg.header.frame_id.data = "laser_frame";
laserScanMsg.header.frame_id.capacity = strlen(laserScanMsg.header.frame_id.data);
laserScanMsg.header.frame_id.size = laserScanMsg.header.frame_id.capacity;

// LaserScan
//laserScanMsg.angle_min = -20 * M_PI / 180; //0.0; // start angle of the scan [rad]
//laserScanMsg.angle_max = 340 * M_PI / 180;//2 * M_PI; // end angle of the scan [rad]

laserScanMsg.range_min = 0.12; // minimum range value [m]
laserScanMsg.range_max = 10.0; // maximum range value [m]

//https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/
laserScanMsg.ranges.capacity = BUF_LEN;
laserScanMsg.ranges.data = memory;
laserScanMsg.ranges.size = 0;

laserScanMsg.intensities.capacity = BUF_LEN;
laserScanMsg.intensities.data = mem_intensity;
laserScanMsg.intensities.size = 0;

delay(2000);

allocator = rcl_get_default_allocator();
Serial.println(F("After allocator()"));

Serial.println(F("Connecting to ROS agent ..."));
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
Serial.println(F("After rclc_support_init()"));

// https://micro.ros.org/docs/tutorials/programming_rcl_rclc/node/
RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support));
Serial.println(F("After rclc_node_init_default()"));

// Creates a reliable rcl publisher
RCCHECK(rclc_publisher_init_default(&publisher, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, LaserScan), TOPIC_NAME));

Serial.println(F("After rclc_publisher_init_*()"));
Serial.print("LiDAR model ");
Serial.println(lidar.getModelName());

Serial.print("LiDAR RX buffer size "); // default 128 hw + 256 sw
Serial.print(LidarSerial.setRxBufferSize(4096)); // must be before .begin()

uint32_t baud_rate = lidar.getSerialBaudRate();
Serial.print(", baud rate ");
Serial.println(baud_rate);

LidarSerial.begin(baud_rate, SERIAL_8N1, LIDAR_RX_PIN, LIDAR_TX_PIN);

lidar.setScanPointCallback(lidar_scan_point_callback);
lidar.setPacketCallback(lidar_packet_callback);
lidar.setSerialWriteCallback(lidar_serial_write_callback);
lidar.setSerialReadCallback(lidar_serial_read_callback);
lidar.setMotorPinCallback(lidar_motor_pin_callback);
//initRanges();
lidar.init();

LDS::result_t result = lidar.start();
Serial.print("LiDAR start() result: ");
Serial.println(lidar.resultCodeToString(result));

if (result < 0)
Serial.println("Is the LiDAR connected to ESP32?");

}

int lidar_serial_read_callback() {
return LidarSerial.read();
}

size_t lidar_serial_write_callback(const uint8_t * buffer, size_t length) {
return LidarSerial.write(buffer, length);
}

void lidar_info_callback(LDS::info_t code, String info) {
Serial.print("LiDAR info ");
Serial.print(lidar.infoCodeToString(code));
Serial.print(": ");
Serial.println(info);
}
void lidar_error_callback(LDS::result_t code, String aux_info) {
Serial.print("LiDAR error ");
Serial.print(lidar.resultCodeToString(code));
Serial.print(": ");
Serial.println(aux_info);
}

void lidar_motor_pin_callback(float value, LDS::lds_pin_t lidar_pin) {
int pin = (lidar_pin == LDS::LDS_MOTOR_EN_PIN) ?
LIDAR_EN_PIN : LIDAR_PWM_PIN;

ledcSetup(LIDAR_PWM_CHANNEL, LIDAR_PWM_FREQ, LIDAR_PWM_BITS);
ledcAttachPin(pin, LIDAR_PWM_CHANNEL);
//ledcAttachChannel(pin, LIDAR_PWM_FREQ, LIDAR_PWM_BITS, LIDAR_PWM_CHANNEL);
digitalWrite(pin, (value == (float)LDS::VALUE_HIGH) ? HIGH : LOW);
// set PWM duty cycle
int pwm_value = ((1<<LIDAR_PWM_BITS)-1)*value;
ledcWrite(LIDAR_PWM_CHANNEL, abs(pwm_value));

}

void lidar_packet_callback(uint8_t * packet, uint16_t length, bool scan_completed) {
return;
}

void lidar_scan_point_callback(float angle_deg, float distance_mm, float quality, bool scan_completed) {
static int i = 0;
static float min=0;
static float max =0;
laserScanMsg.ranges.size = BUF_LEN;
laserScanMsg.intensities.size = BUF_LEN;

if (i % 1 == 0) {
if(i==0){
min=angle_deg;

  }
  //Serial.println(angle_deg);
  if(i<1000){
  laserScanMsg.ranges.data[i] = distance_mm/1000;
  //laserScanMsg.intensities.data[i] = distance_mm/1000;
  }

}
i++;

if (scan_completed) {

  max=angle_deg;

  clock_gettime(CLOCK_REALTIME, &tv);         //main/time_sync.c
  laserScanMsg.header.stamp.sec = tv.tv_sec;
  laserScanMsg.header.stamp.nanosec = tv.tv_nsec;
  tv_new = tv.tv_nsec/ 1.0e9;
  laserScanMsg.time_increment = (tv_new - tv_old)/i;
  laserScanMsg.scan_time = (tv_new - tv_old);
  tv_old = tv.tv_nsec/1.0e9;
  
  // LaserScan  
  laserScanMsg.angle_min = (min-360) * M_PI / 180; //0.0;               // start angle of the scan [rad]
  laserScanMsg.angle_max = max * M_PI / 180;//2 * M_PI;           // end angle of the scan [rad]
  laserScanMsg.angle_increment = (laserScanMsg.angle_max - laserScanMsg.angle_min) / i; // rad/meas
  
  
  rcl_ret_t rc = rcl_publish(&publisher, &laserScanMsg, NULL);
  if (rc == RCL_RET_OK)
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  else
  Serial.println("rcl_publish() error");

   i = 0;
  }

}

void loop() {

lidar.loop();
}

[kaiaai]

The code looks fine. That said, there should be no 4-5 degree or any visible wobbling at all. My X4 has always been rock stable (as long as the Lidar is NOT turning around its Z axis). So the only reason to add PID for X4 is to set its rotation speed to something other than its default. About X4 sending -20 to 340 degree data with respect to scan_complete, this is typical for some low-cost Lidars. Sometimes 0 degree reading does not even align with the physical Lidar's Y axis (you have to add a calibration offset offset).

…

On Wed, Jan 1, 2025, 11:28 AM Skara77 ***@***.***> wrote: I am checking scan_completed with your example; ydlidar_x4_esp32.ino. I don't have other lidar to check it. But works fine when connected to pc with usb adapter. In my code, rviz2 scan image 4 - 5 degree shifting. This is my code; #include <micro_ros_arduino.h> #include <stdio.h> #include <rcl/rcl.h> #include <rcl/error_handling.h> #include <rclc/rclc.h> #include <rclc/executor.h> #include <sensor_msgs/msg/laser_scan.h> #include <LDS_YDLIDAR_X4.h> #if !defined(ESP32) #error This example is only available for ESP32 Dev module #endif #define FRAME_ID "laser_frame" #define TOPIC_NAME "scan" #define NODE_NAME "uros_node" #define BUF_LEN 1000 #define LED_PIN 2 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}} #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}} const uint8_t LIDAR_EN_PIN = 19; // ESP32 Dev Kit LiDAR enable pin const uint8_t LIDAR_PWM_PIN = 15; // LiDAR motor speed control using PWM const uint8_t LIDAR_TX_PIN = 17; const uint8_t LIDAR_RX_PIN = 16; #define LIDAR_PWM_FREQ 10000 #define LIDAR_PWM_BITS 11 #define LIDAR_PWM_CHANNEL 2 // ESP32 PWM channel for LiDAR motor speed control HardwareSerial LidarSerial(1); LDS_YDLIDAR_X4 lidar; rcl_publisher_t publisher; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; sensor_msgs__msg__LaserScan laserScanMsg; struct timespec tv = {0}; float tv_old = 0; float tv_new = 0; static float memory[BUF_LEN]; static float mem_intensity[BUF_LEN]; static float dist_offset = 0.15; void error_loop(){ Serial.println(F("error_loop()")); while(1){ digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(500); } } void setup() { Serial.begin(921600); set_microros_wifi_transports((char*)"Wifi", (char*)"Password", (char*)"192.168.1.200", 8888); pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, HIGH); memset((void*)&laserScanMsg, 0, sizeof(laserScanMsg)); laserScanMsg.header.frame_id.data = "laser_frame"; laserScanMsg.header.frame_id.capacity = strlen(laserScanMsg.header.frame_id.data); laserScanMsg.header.frame_id.size = laserScanMsg.header.frame_id.capacity; // LaserScan //laserScanMsg.angle_min = -20 * M_PI / 180; //0.0; // start angle of the scan [rad] //laserScanMsg.angle_max = 340 * M_PI / 180;//2 * M_PI; // end angle of the scan [rad] laserScanMsg.range_min = 0.12; // minimum range value [m] laserScanMsg.range_max = 10.0; // maximum range value [m] //https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/ laserScanMsg.ranges.capacity = BUF_LEN; laserScanMsg.ranges.data = memory; laserScanMsg.ranges.size = 0; laserScanMsg.intensities.capacity = BUF_LEN; laserScanMsg.intensities.data = mem_intensity; laserScanMsg.intensities.size = 0; delay(2000); allocator = rcl_get_default_allocator(); Serial.println(F("After allocator()")); Serial.println(F("Connecting to ROS agent ...")); RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); Serial.println(F("After rclc_support_init()")); // https://micro.ros.org/docs/tutorials/programming_rcl_rclc/node/ RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support)); Serial.println(F("After rclc_node_init_default()")); // Creates a reliable rcl publisher RCCHECK(rclc_publisher_init_default(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, LaserScan), TOPIC_NAME)); Serial.println(F("After rclc_publisher_init_*()")); Serial.print("LiDAR model "); Serial.println(lidar.getModelName()); Serial.print("LiDAR RX buffer size "); // default 128 hw + 256 sw Serial.print(LidarSerial.setRxBufferSize(4096)); // must be before .begin() uint32_t baud_rate = lidar.getSerialBaudRate(); Serial.print(", baud rate "); Serial.println(baud_rate); LidarSerial.begin(baud_rate, SERIAL_8N1, LIDAR_RX_PIN, LIDAR_TX_PIN); lidar.setScanPointCallback(lidar_scan_point_callback); lidar.setPacketCallback(lidar_packet_callback); lidar.setSerialWriteCallback(lidar_serial_write_callback); lidar.setSerialReadCallback(lidar_serial_read_callback); lidar.setMotorPinCallback(lidar_motor_pin_callback); //initRanges(); lidar.init(); LDS::result_t result = lidar.start(); Serial.print("LiDAR start() result: "); Serial.println(lidar.resultCodeToString(result)); if (result < 0) Serial.println("Is the LiDAR connected to ESP32?"); } int lidar_serial_read_callback() { return LidarSerial.read(); } size_t lidar_serial_write_callback(const uint8_t * buffer, size_t length) { return LidarSerial.write(buffer, length); } void lidar_info_callback(LDS::info_t code, String info) { Serial.print("LiDAR info "); Serial.print(lidar.infoCodeToString(code)); Serial.print(": "); Serial.println(info); } void lidar_error_callback(LDS::result_t code, String aux_info) { Serial.print("LiDAR error "); Serial.print(lidar.resultCodeToString(code)); Serial.print(": "); Serial.println(aux_info); } void lidar_motor_pin_callback(float value, LDS::lds_pin_t lidar_pin) { int pin = (lidar_pin == LDS::LDS_MOTOR_EN_PIN) ? LIDAR_EN_PIN : LIDAR_PWM_PIN; ledcSetup(LIDAR_PWM_CHANNEL, LIDAR_PWM_FREQ, LIDAR_PWM_BITS); ledcAttachPin(pin, LIDAR_PWM_CHANNEL); //ledcAttachChannel(pin, LIDAR_PWM_FREQ, LIDAR_PWM_BITS, LIDAR_PWM_CHANNEL); digitalWrite(pin, (value == (float)LDS::VALUE_HIGH) ? HIGH : LOW); // set PWM duty cycle int pwm_value = ((1<<LIDAR_PWM_BITS)-1)*value; ledcWrite(LIDAR_PWM_CHANNEL, abs(pwm_value)); } void lidar_packet_callback(uint8_t * packet, uint16_t length, bool scan_completed) { return; } void lidar_scan_point_callback(float angle_deg, float distance_mm, float quality, bool scan_completed) { static int i = 0; static float min=0; static float max =0; laserScanMsg.ranges.size = BUF_LEN; laserScanMsg.intensities.size = BUF_LEN; if (i % 1 == 0) { if(i==0){ min=angle_deg; } //Serial.println(angle_deg); if(i<1000){ laserScanMsg.ranges.data[i] = distance_mm/1000; //laserScanMsg.intensities.data[i] = distance_mm/1000; } } i++; if (scan_completed) { max=angle_deg; clock_gettime(CLOCK_REALTIME, &tv); //main/time_sync.c laserScanMsg.header.stamp.sec = tv.tv_sec; laserScanMsg.header.stamp.nanosec = tv.tv_nsec; tv_new = tv.tv_nsec/ 1.0e9; laserScanMsg.time_increment = (tv_new - tv_old)/i; laserScanMsg.scan_time = (tv_new - tv_old); tv_old = tv.tv_nsec/1.0e9; // LaserScan laserScanMsg.angle_min = (min-360) * M_PI / 180; //0.0; // start angle of the scan [rad] laserScanMsg.angle_max = max * M_PI / 180;//2 * M_PI; // end angle of the scan [rad] laserScanMsg.angle_increment = (laserScanMsg.angle_max - laserScanMsg.angle_min) / i; // rad/meas rcl_ret_t rc = rcl_publish(&publisher, &laserScanMsg, NULL); if (rc == RCL_RET_OK) digitalWrite(LED_PIN, !digitalRead(LED_PIN)); else Serial.println("rcl_publish() error"); i = 0; } } void loop() { lidar.loop(); } — Reply to this email directly, view it on GitHub <#11 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AIAIQ5MFYGVDAHKUCIG6GWT2IQ6WTAVCNFSM6AAAAABUOPBQF6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDKNRXGEZDAMJWGU> . You are receiving this because you commented.Message ID: ***@***.***>

[Skara77]

ok thanks, I think issue with cpu load. when no load, -13 to 347 degree, without any change. When cpu load increase with micro ros, it deviates -20 to 340, -15 to 345, -13 to 347. This angle deviation also creates deviation in rviz.