Rplidar A2 屏蔽固定角度
Rplidar A2 的扫描角度图:
最近在使用Rplidar进行AMCL导航的时候,发现一个问题。由于雷达是固定在车的前面,雷达后面一部分位置被云台给遮挡住了。如图所示:
我们都知道,在ros的AMCL里,扫描得到的障碍区域都是对应一个膨胀系数的。这时为了让我们的车子在导航时,可以更好的避开。但是雷达被遮挡带来的一个缺点就是,当我们在amcl导航时,由于膨胀系数的原因,后面的遮挡就会膨胀到遮住整辆车。从而导致路径规划失败。所以必须舍弃一部分的雷达扫描角度。
回到代码中
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);
紧接着修改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);
}
修改好之后,重新编译应该就可以了。