思岚科技Rplidar A3实现指定角度扫描及扫描结果存储输出

思岚科技Rplidar A3实现指定任意角度扫描及扫描结果.txt存储输出

1. 前言
   思岚科技Rplidar系列产品非常不错，拥有A1、A2、A3系列成熟的商业激光雷达产品。产品均可从官网获取相应的SDK和开发指导文档。思岚科技Rplidar实现360°测量，但是在实际使用中，由于机构、使用场景等因素，时常要求实现指定角度角度扫描。
   本文根据SDK实现指定角度测量及数据输出，经过验证，功能完善，效果稳定。
   博客内容经过验证，详实可靠。
   有些网友留言编译错误，一般是头文件问题或者特定函数定义问题，建议大家先看一下
   思岚科技RPlidar A3激光雷达ROS源码详解https://blog.csdn.net/qq_41854650/article/details/102665317
   在此基础上再来看篇博客。

2. rplidar_ros坐标系说明
   rplidar是按照顺时针旋转，SDK数据输出是带距离和角度信息的左手系数据，rplidar_ros输出已经将其转化为右手坐标系输出。雷达数据坐标系参照雷达数据线的位置（A1）/形状（A2）和下面的图示判断。
   
   思岚科技官方提供的rplidar_ros-master1.10.0，这一版本中，SDK默认输出的雷达数据按照（-180°，180°）沿着逆时针方向输出。
   本文对角度和输出顺序进行了修正，实现0°-360°顺时针方向输出，通过参数设置，实现任意角度扫描。

3. 本文实现功能
   （1）实现0°-360°顺时针扫描数据输出。SDK源码输出数据是-180°-180°逆时针方向数据输出，即先扫描后输出。实际使用中往往需要先扫描先输出，即输出的扫描数据时序与雷达运动时序一致。本文通过代码修改实现了0°-360°顺时针扫描数据输出，即先扫描先输出。
   （2）实现0°-360°范围内，任意角度扇形区域切割扫描。由于rplidar是360°扫描，实际使用中经常遇到我们对特定角度的扫描数据感兴趣，其他角度的数据需要舍弃，本文通过修改代码实现。只需指定起点角度和终点角度，即可切割两点之间的扇形区域。
   （3）实现测试数据.txt格式输出。通过代码实现测试数据保存为.txt格式输出。

4. 代码实现
   涉及更改node.cpp和client.cpp，具体如下：
   node.cpp重构void publish_scan函数，增加角度判断模块

//雷达发布消息子函数
void publish_scan(
    ros::Publisher *pub, rplidar_response_measurement_node_hq_t *nodes, size_t node_count,
    ros::Time start, 
    double scan_time, bool inverted,  
    float angle_min, float angle_max, 
    float max_distance,              
    std::string frame_id,            
    bool cut_angle, int first_point_degree,
    int second_point_degree) 
{
  static int scan_count = 0;
  sensor_msgs::LaserScan scan_msg; 
  scan_msg.header.stamp = start; 
  scan_msg.header.frame_id = frame_id; 
  scan_count++;                    
  bool reversed =(angle_max > angle_min);
  if (reversed) {
    scan_msg.angle_min =
        M_PI - angle_max;
    scan_msg.angle_max = M_PI - angle_min;
  } else {
    scan_msg.angle_min = M_PI - angle_min;
    scan_msg.angle_max = M_PI - angle_max;
  }
  //角度修正，从小到大
  scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) /(double)(node_count - 1); //扫描精度
  scan_msg.scan_time = scan_time;                      //扫描时间开始
  scan_msg.time_increment = scan_time / (double)(node_count - 1); //时间间隔
  scan_msg.range_min = 0.15;         //最小的范围
  scan_msg.range_max = max_distance; // 8.0;
  scan_msg.intensities.resize(node_count); //信号质量
  scan_msg.ranges.resize(node_count);      //范围
  if (!cut_angle) { //不进行角度裁剪
    bool reverse_data = (!inverted && reversed) || (inverted && !reversed); //是否翻转数据
    // rplidar是按照顺时针旋转，SDK数据输出是带距离和角度信息的左手系数据，rplidar_ros输出已经将其转化为右手坐标系输出。
    if (!reverse_data) { 
      for (size_t i = 0; i < node_count; i++) {
        float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000;
        if (read_value == 0.0)
          scan_msg.ranges[i] = std::numeric_limits<
              float>::infinity(); //返回编译器允许的float类型无穷大
        else
          scan_msg.ranges[i] = read_value;
        scan_msg.intensities[i] = (float)(nodes[i].quality >> 2);
      }
    } else { //思岚默认输出方式，进行数据翻转，输出数据符合右手坐标系，数据按照逆时针方向发送，rviz成像与现实相同
      for (size_t i = 0; i < node_count; i++) {
        float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000;
        if (read_value == 0.0)
          scan_msg.ranges[node_count - 1 - i] =
              std::numeric_limits<float>::infinity();
        else
          scan_msg.ranges[node_count - 1 - i] = read_value;
        scan_msg.intensities[node_count - 1 - i] =
            (float)(nodes[i].quality >> 2);
      }
    }

  } else { //进行角度裁剪

    // 180°<second_point_degree<360°,0°<first_point_degree<180°,
    //裁剪出second_point_degree与first_point_degree顺时针形成的扇形区域。
    //先全部置inf，注意：如果初始化是0,则表示范围内无障碍，故不能置0。inf表示无数据。
    //注意，如果进行SLAM建图，无数据输入，会导致角度跳变过大，系统报错，如hector:SearchDir angle change too large.
    //采取的方案设被裁减的角度设置默认值为rplidar最小测量值0.15米。   
    for (size_t i = 0; i < node_count; i++) {
      scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
      // scan_msg.ranges[i] =0.15;
    }

    bool reverse_data = (!inverted && reversed) || (inverted && !reversed);
    if (!reverse_data) {
      for (size_t i = 0; i < node_count; i++) {
        float get_degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * i) + 180;
        // std::cout<<"degree="<<degree<<std::endl;
        if(get_degree > second_point_degree || get_degree < first_point_degree)
        //裁剪角度，获得(0°，first_point_degree）和(second_point_degree,360°)扇形区域
        //if (get_degree >= first_point_degree && get_degree <= second_point_degree) 
        //裁剪角度，获得(second_point_degree,first_point_degree)扇形区域
        {
          float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; 
          if (read_value == 0.0)
            scan_msg.ranges[i] = std::numeric_limits< float>::infinity(); 
          else
            scan_msg.ranges[i] = read_value; 
          scan_msg.intensities[i] = (float)(nodes[i].quality >> 2);
        }
      }
    } else {
      for (size_t i = 0; i < node_count; i++) {
        float get_degree =  RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * i) + 180;
        if(get_degree > second_point_degree || get_degree < first_point_degree)
        //裁剪角度，获得(0°，first_point_degree）和(second_point_degree,360°)扇形区域
        //if (get_degree >= first_point_degree && get_degree <= second_point_degree) 
        //裁剪角度，获得(first_point_degree,second_point_degree)扇形区域
        {
          float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; 
          if (read_value == 0.0)
            scan_msg.ranges[node_count - 1 - i] =
                std::numeric_limits<float>::infinity();
          else
            scan_msg.ranges[node_count - 1 - i] = read_value;
            scan_msg.intensities[node_count - 1 - i] = (float)(nodes[i].quality >> 2); 
        }
      }
    }
  }
 
  //扫描结果发布出去
  pub->publish(scan_msg);
}

main函数新增变量，并通过.launch文件传入：

   bool cut_angle = false; //角度切割标志位
  int first_point_degree = 90;
  int second_point_degree = 270;
  nh_private.param<bool>("cut_angle", cut_angle, false);
  if (cut_angle) {
    nh_private.param<int>("second_point_degree", second_point_degree, 180);
    nh_private.param<int>("first_point_degree", first_point_degree, 180);
  }

rplidar_a3.launch和test_rplidar_a3.launch文件增加配置参数：

 <param name="cut_angle"    type="bool"   value="true"/>
 <param name="first_point_degree"    type="int"   value="90"/>
 <param name="second_point_degree"    type="int"   value="270"/>

client.cpp

void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan) 
{
  int count = scan->scan_time / scan->time_increment;
  ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(),
           count);
  ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min),
           RAD2DEG(scan->angle_max));
 std::ofstream outfile("path/rplidar_data.txt");//open()打开一个txt
  for (int i = 0; i < count; i++) {
    float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i)+180;
    ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);  
    outfile << "i=" << i <<"  "<< "degree: " << degree <<"  "<<"scan->ranges[i]: " << scan->ranges[count-1-i] << "\n";//写入信息
  }
  	outfile.close();
}