Ros

N10（镭神雷达）

ancientcc
  • 目前只支持N10（lidar_name=N10）。其它型号代码有保留，但没实物测过。为安全，一开始有验证lidar_name参数必须填“N10”。
  • 按rose_ros对雷达驱动要求，一定发布sensor_msgs::LaserScan类型话题（pubScan=true），话题名“/laser_scan”（scan_topic），消息中frame_id填“laser”（frame_id）。
  • 一定不发布sensor_msgs::PointCloud2类型话题（pubPointCloud2=false）。cartographer_node节点会发布这话题，而且已带上机器人位姿。雷达驱动填充这个消息还消耗时间。
  • 取消专门用于发布“laser_scan”话题的线程pubscan_thread_。在node线程解析出一帧点云后，就调用发布函数pubScanThread。我不认为发布话题要用个专门线程。
  • 有控制电机操作。运行雷达驱动时，会启动电机；结束雷达驱动时，会停转电机。

驱动源码在小程序Basic项目：<libleagor_basic>/lsx10。以下是一段点云(sensor_msgs::LaserScan)日志。可看到一秒大概10帧点云，即扫描频率10HZ。

SDL_Log("%u HandleLaserScanMessage, ranges.size: %i range(%.2f, %.2f) scan_time: %.3f angle(%.3f, %.3f, %.3f)", 
      SDL_GetTicks(), (int)msg->ranges.size(), msg->range_min, msg->range_max, msg->scan_time, 
      RAD2DEG(msg->angle_min), RAD2DEG(msg->angle_increment), RAD2DEG(msg->angle_max));
---
37454 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.801, 180.000)
37554 HandleLaserScanMessage, ranges.size: 449 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.803, 180.000)
37652 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.098 angle(-180.000, 0.801, 180.000)
37752 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.801, 180.000)
37851 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.801, 180.000)
37955 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.103 angle(-180.000, 0.801, 180.000)
38054 HandleLaserScanMessage, ranges.size: 451 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.800, 180.000)
38154 HandleLaserScanMessage, ranges.size: 449 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.803, 180.000)
38254 HandleLaserScanMessage, ranges.size: 450 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.801, 180.000)
38354 HandleLaserScanMessage, ranges.size: 451 range(0.10, 12.00) scan_time: 0.099 angle(-180.000, 0.800, 180.000)
---

range(0.10, 12.00)指的是sensor_msgs::LaserScan中的range_min、range_max字段。它们是“主观”指定，这里设为(0.10, 12.00)是和思岚A1M8一样。

一、控制电机

图1 控制电机（摘自“N10数据输出协议V1.1.pdf”）

 

运行雷达驱动时，会启动电机，只会启动一次。结束雷达驱动时，会停转电机。

void LslidarDriver::ctrl_motor(bool start)
{
	VALIDATE(valid(), null_str);

	if (lidar_name == "N10") {
		const int len = 43 * 4 + 16;
		uint8_t data[len] = {0xa5, 0x5a, 0x55, 0x0};
		data[len - 1] = 0xfb;
		data[len - 2] = 0xfa;
		data[len - 3] = start? 0x01: 0x00;
		data[len - 4] = 0x01;
		send_data(data, len);
	}
}

void LslidarDriver::did_read_one_packet(const uint8_t* packet_bytes, int len)
{
	......

	if (!is_start) {
		ctrl_motor(true);
		is_start = true;
	}
	...
}

启动电机发生在驱动读取到第一帧雷达点云数据时。发现is_start是false，就启动电机。电机只启动一次。这里有个问题，此次启动电机会不会出现失败？——曾在pdf找某个能反映电机转速的反馈，设想由反馈知道还没转，那就再发。只找到个或许有联系的“转速信息”，见图2“Speed_H、Speed_L”。手头这个雷达测试下来，电机转动时，Speed是100ms左右，这个pdf写的10HZ差不多。电机不转时，Speed是150ms左右。我不理解这个数值意义，也就没用。

图2 输出点云协议（摘自“N10数据输出协议V1.1.pdf”）

 

二、sensor_msgs::LaserScan中的ranges可能全是inf

图3 ranges全inf

即使ranges全是inf，cartographer会正确处理掉这种情況。HandleLaserScanMessage处理点云时，调用ToPointCloudWithIntensities，把sensor_msgs::LaserScan格式的点云转成carto::sensor::PointCloudWithIntensities格式的points，接下调用void SensorBridge::HandleLaserScan。

void SensorBridge::HandleLaserScan(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,
    const carto::sensor::PointCloudWithIntensities& points) {
  if (points.points.empty()) {

points.points数就是msg.ranges的单元数。这里是0，退出。对此次点云处理就结束了。

    return;
  }
  ...
}

修改雷达驱动，可避免输出全inf。既然后面能安全处理，就暂不改了。

 

三、官方源码函数：LslidarDriver::receive_data

receive_data用于从雷达获取一帧点云数据。一旦成功，参数package_bytes存储着这帧点云数据，返回值表示字节数。如果失败，返回值是0，packet_bytes填什么不确定。过程中用了四次while读。

  1. 读出第一个字节0xa5。见图2的“Byte_0”。
  2. 到现在count是1。读出第二个字节0x5a。见图2的“Byte_1”。
  3. 到现在count是2。读出第三、第四个字节，见图2的“Byte_2、Byte_3”。对一些型号雷达，当中有表示该次点云字节数。像N10，Byte_2存储点云长度，值58。至此要算出一个表示此帧点云字节数的变量len，对N10，len等于58。
  4. 到现在count是4。此次读出该点云剩余字节数据len - count。对N10，要读54字节。不能多，也不能少。
/lslidar_driver/src/lslidar_driver.cc
------
int LslidarDriver::receive_data(unsigned char *packet_bytes)
{
	int link_time = 0;
	int len_H = 0;
	int len_L = 0;
	int len = 0;
	int count_2 = 0;
	int count = 0;
	while (count <= 0)
	{
		count = serial_->read(packet_bytes, 1);
		LslidarDriver::recvThread_crc(count, link_time);
	}
	if (packet_bytes[0] != 0xA5)
		return 0;

第一次while读。读出第一个字节0xa5。

	while (count_2 <= 0)
	{
		count_2 = serial_->read(packet_bytes + count, 1);
		if (count_2 >= 0)
			count += count_2;
		LslidarDriver::recvThread_crc(count_2, link_time);
	}

	count_2 = 0;
	if (packet_bytes[1] != 0x5A)
		return 0;

第二次while读。到现在count是1。读出第二个字节0x5a。

	while (count_2 <= 0)
	{
		count_2 = serial_->read(packet_bytes + count, 2);
		if (count_2 >= 0)
			count += count_2;
		LslidarDriver::recvThread_crc(count_2, link_time);
	}

	count_2 = 0;

	if (lidar_name == "M10")
		len = 92;
	else if (lidar_name == "M10_GPS")
		len = 102;
	else if (lidar_name == "N10_P")
		len = 108;
	else if (lidar_name == "N10" || lidar_name == "L10")
		len = packet_bytes[2];
	else
	{
		len_H = packet_bytes[2];
		len_L = packet_bytes[3];
		len = len_H * 256 + len_L;
	}
	if (lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" 
  	    || lidar_name == "M10_PLUS")
	{
		if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00)
			len = 188;
	}

第三次while读。到现在count是2。读出第三、第四个字节，对一些型号雷达，当中有表示该次点云字节数，像N10。然后得到此帧点云长度len，N10是58，即图2中Byte_2的Legnth值是58。

	while (count < len)
	{
		count_2 = serial_->read(packet_bytes + count, len - count);
		if (count_2 >= 0)
			count += count_2;
		LslidarDriver::recvThread_crc(count_2, link_time);
	}

第四次while。到现在count是4，此次读出该点云剩余字节数据“len - count”。对N10，还要读54字节。不能多，也不能少。

	if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P")
	{
		if (packet_bytes[PACKET_SIZE - 1] != N10_CalCRC8(packet_bytes, PACKET_SIZE - 1))
			return 0;
	}
	return len;
}

返回值len是此次点云帧字节数，N10时58。

发布于 2023-12-07 20:49:07

全部评论: 0 条

    写评论: