2D激光雷达数据切割

本文主要介绍如何将2D激光雷达数据切割成指定角度的数据。

参数设置

在config中的yaml文件中修改参数，主要包括雷达原始数据的topic名称，拟发布的切割角度，数据类型multechoflag等。

程序编译及运行

1. 将代码放入src文件夹中；
2. 命令行输入catkin_make即可完成程序编译。
3. 程序运行

source devel/setup.bash
roslanch lidar_crop LidarCrop_XX.launch

主程序

 int main(int argc, char **argv)
{
  ros::init(argc, argv, "lidarcrop");
  ros::NodeHandle n;
  bool multechoflag,simulation;
  int lidarnum;
  //接收原始数据，在config文件中的.yaml中修改
  n.getParam("lidarnum",lidarnum);
  n.getParam("multechoflag",multechoflag);
  n.getParam("simulation",simulation);
  LidarCrop_test lidarGroup[100];
  for(int i=0;i<lidarnum;i++){
    std::string lidarsubname="subtopicname"+std::to_string(i);
    std::string lidarpubname="pubtopicname"+std::to_string(i);
    std::string lidarangle="angle"+std::to_string(i);
    
    n.getParam(lidarsubname,lidarGroup[i].SubTopic);
    n.getParam(lidarpubname,lidarGroup[i].PubTopic);
    n.getParam(lidarangle,lidarGroup[i].CropAngle);
  }
  for(int i=0;i<lidarnum;i++){
  if(!multechoflag){
      if(simulation){
      lidarGroup[i].PubCropedPoint=n.advertise<sensor_msgs::LaserScan>(lidarGroup[i].PubTopic,10);
      lidarGroup[i].SubOrignPoint=n.subscribe<sensor_msgs::LaserScan>(lidarGroup[i].SubTopic,1000,std::bind(&LidarCrop_test::callbackfunctionsimulation,&lidarGroup[i],std::placeholders::_1));
      }
      else{
      lidarGroup[i].PubCropedPoint=n.advertise<sensor_msgs::LaserScan>(lidarGroup[i].PubTopic,10);
      lidarGroup[i].SubOrignPoint=n.subscribe<sensor_msgs::LaserScan>(lidarGroup[i].SubTopic,1000,std::bind(&LidarCrop_test::callbackfunction,&lidarGroup[i],std::placeholders::_1));
      }}
 else{
      lidarGroup[i].PubCropedPoint=n.advertise<sensor_msgs::MultiEchoLaserScan>(lidarGroup[i].PubTopic,10);
      lidarGroup[i].SubOrignPoint=n.subscribe<sensor_msgs::MultiEchoLaserScan>(lidarGroup[i].SubTopic,1000,std::bind(&LidarCrop_test::callbackfunctionMultiEcho,&lidarGroup[i],std::placeholders::_1));

  } }
   ros::spin();

  return 0;
}

LaserScan数据切割

主要处理代码：

void callbackfunction(const sensor_msgs::LaserScan::ConstPtr &msg){
          sensor_msgs::LaserScan msg1;
          msg1.header=msg->header;
          msg1.angle_min=-CropAngle/360*3.1415926;
          msg1.angle_max=CropAngle/360*3.1415926;
          msg1.angle_increment=msg->angle_increment;
          msg1.time_increment=msg->time_increment;
          msg1.scan_time=msg->scan_time;
          msg1.range_max=msg->range_max;
          msg1.range_min=msg->range_min;
          msg1.ranges.resize(msg->ranges.size());
          msg1.intensities.resize(msg->intensities.size());
          int count=0;
          int num=CropAngle/2;
          int num1=msg1.ranges.size();
          
          for(int i=num1-num;i<num1;i++){
               msg1.ranges[i-num1+num]=msg->ranges[i];
               if(msg->intensities.size()>i){
                  msg1.intensities[i-num1+num]=msg->intensities[i];
               }
               count++;
          }
         for(int i=0;i<num;i++){
               msg1.ranges[i+num]=msg->ranges[i];
               if(msg->intensities.size()>i){
                  msg1.intensities[i+num]=msg->intensities[i];
               }
               count++;
            }
           msg1.ranges.resize(count);

        if(msg->intensities.size() >= count){
         msg1.intensities.resize(count);}
         
         PubCropedPoint.publish(msg1);     
   }    

MultiEchoLaserScan数据切割

主要处理代码：

 void callbackfunctionMultiEcho(const sensor_msgs::MultiEchoLaserScan::ConstPtr &msg){
          sensor_msgs::MultiEchoLaserScan msg1;
          msg1.header=msg->header;
          msg1.angle_min=-CropAngle/360*3.1415926;
          msg1.angle_max=CropAngle/360*3.1415926;
          msg1.angle_increment=msg->angle_increment;
          msg1.time_increment=msg->time_increment;
          msg1.scan_time=msg->scan_time;
          msg1.range_max=msg->range_max;
          msg1.range_min=msg->range_min;
          msg1.ranges.resize(msg->ranges.size());
          msg1.intensities.resize(msg->intensities.size());
          int num1=msg->ranges.size();
          int count=0;
          for(int i=0;i<num1;i++){
            if((msg->angle_min+i*msg->angle_increment>=msg1.angle_min)&&(msg->angle_min+i*msg->angle_increment<=msg1.angle_max)){
                   msg1.ranges[count]=msg->ranges[i];
                   if(msg->intensities.size()>i){
                  msg1.intensities[count]=msg->intensities[i];
               }
               count++;
            }
          }
          msg1.ranges.resize(count);
        if(msg->intensities.size() >= count){
         msg1.intensities.resize(count);}
         PubCropedPoint.publish(msg1);     
   } 

};

参考代码