1. 程式人生 > >鐳射雷達掃描角度裁剪

鐳射雷達掃描角度裁剪

    採用的是思嵐的鐳射雷達A1,由於base的機構設計,需要對雷達進行角度裁剪。rplidar A1的掃描角度的定義如下:

 

    參考博文:https://blog.csdn.net/lu199012/article/details/73457209

我需要的掃描範圍是【0,π】,在ROS中,順時針為負,逆時針為正,裁剪方法:在node.cpp中void publish_scan()函式中新增圖中的角度比較的語句即可.

               scan_msg.ranges[node_count-1-i] = read_value;
            scan_msg.intensities[node_count-1-i] = (float) (nodes[i].sync_quality >> 2);
        }
    }
 //  printf("------------scan_msg.ranges.size()=%d\n",scan_msg.ranges.size());//
#if 1
    for(int i = 0;i<scan_msg.ranges.size();i++)
        {
                //if(((scan_msg.angle_min+i*scan_msg.angle_increment) > M_PI)&&((scan_msg.angle_min+i*scan_msg.angle_increment) < M_PI*2))
                if(((scan_msg.angle_min+i*scan_msg.angle_increment) > -M_PI)&&((scan_msg.angle_min+i*scan_msg.angle_increment) < 0))
                        scan_msg.ranges[i]=std::numeric_limits<float>::infinity();
        } //add by lcw
#endif
    pub->publish(scan_msg);
}

裁剪後slam建圖的效果如下,在rviz會顯示想要的分割平面,圖1顯示裁剪後只掃描180°的範圍,圖2顯示裁剪後的建圖效果。 

 

下圖是裁剪之前slam建圖的效果,與上圖進行對比。