rplidar 掃描角度設置

參考網站::   https://blog.csdn.net/sunyoop/article/details/78302090   

https://blog.csdn.net/dzhongjie/article/details/84575482 

通常我們選取是laser 正前方的扇型數據，從上圖可以看出
例如正面180度扇型數據，那么選取的度數為0~90，270~359的數據
270度面的扇型數據，選取度數為0~135， 225~359的數據
回到代碼中

1.下載rplidar node的源碼https://github.com/robopeak/rplidar_ros
2.打開node.cpp文件，簡單看下邏輯

 

修改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)
{

    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 = 8.0;

    scan_msg.intensities.resize(node_count);
    scan_msg.ranges.resize(node_count);
    bool reverse_data = (!inverted && reversed) || (inverted && !reversed); //修改后的代碼reverse_data就沒有用處了。

    /* 將rplidar放到hokuyo的位置，角度信息見上面的圖如下
                  0度/前
      270度/左   rplidar的方向  90度/右    
                 180度/后
      kobuki接收到 LaserScan scan_msg.ranges數據對應的角度信息
                  180度/前
      270度/左   kobuki的方向  90度/右    
                  0度/后

    要把 0～90度對應的node數據映射到 180～90度的scan_msg.ranges中
    要把 90～180度對應的node數據映射到 90～0度的scan_msg.ranges中
    要把 180～270度對應的node數據映射到 359～270度的scan_msg.ranges中
    要把 270～359度對應的node數據映射到 270～180度的scan_msg.ranges中

    */
    const size_t degree_90 = 90; //固定值，算法需要
    const size_t degree_270 = 270; //固定值，算法需要
    const size_t left_degrees = 225; // 裁剪的范圍 保留數據225～359. 
    const size_t right_degrees = 135; // 裁剪的范圍 保留數據0～135. 
    //先全部置inf，注意：如果初始化是0,則表示范圍內無障礙，故不能置0。inf表示無數據 
    for (size_t i = 0; i < node_count; i++){ 
        scan_msg.ranges[i] = std::numeric_limits<float>::infinity(); 
    } 

    //將數據分別對應設置進去 
   for (size_t i = 0; i < node_count; i++)
    {
        float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000; 
        if (i < right_degrees)
        { 
           if (read_value == 0.0) 
               scan_msg.ranges[2*degree_90 - i] = std::numeric_limits<float>::infinity(); 
           else 
               scan_msg.ranges[2*degree_90 - i] = read_value; 

           scan_msg.intensities[2*degree_90 - i] = (float) (nodes[i].quality >> 2); 

        } 
        else if (i > left_degrees) 
        { 
           if (read_value == 0.0) 
        scan_msg.ranges[2*degree_270 - i] = std::numeric_limits<float>::infinity(); 
           else 
                scan_msg.ranges[2*degree_270 - i] = read_value; 

           scan_msg.intensities[2*degree_270 - i] = (float) (nodes[i].quality >> 2); 
        } 
        else 
        { 
           //do nothing; 
        } 
    } 
    //發布出去 
    pub->publish(scan_msg);
}

 

講需要裁剪的角度放到launch文件中，當作參數傳入，比在代碼中修改好很多

例如：在rplidar.launch文件中加入

<param name="cut_angle"    type="bool"   value="true"/>

<param name="right_degrees"    type="int"   value="90"/>

<param name="left_degrees"    type="int"   value="270"/>

然后在main（）函數中添加參數：

bool cut_angle =false;
int right_degrees=180;
int left_degrees=180; 
 nh_private.param<bool>("cut_angle", cut_angle, false);
 if (cut_angle){
     nh_private.param<int>("left_degrees", left_degrees, 180);
      nh_private.param<int>("right_degrees", right_degrees, 180);
}

講需要裁剪的角度放到launch文件中，當作參數傳入，比在代碼中修改好很多

例如：在rplidar.launch文件中加入

<param name="cut_angle"    type="bool"   value="true"/>

<param name="right_degrees"    type="int"   value="90"/>

<param name="left_degrees"    type="int"   value="270"/>

重新編譯即可。

 （注：：不知道為什么Boost模式下 運行報錯，A2 standard Express模式沒有問題）