回调函数:
// 回调函数 进行数据处理
void GMapping::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
static ros::Time last_map_update(0, 0); //存储上一次地图更新的时间
if (!got_first_scan_) //找第一帧的scan
{
// 将雷达各个角度的sin与cos值保存下来,以节约计算量
CreateCache(scan_msg);
got_first_scan_ = true; //改变第一帧的标志位
}
start_time_ = std::chrono::steady_clock::now();
// 计算当前雷达数据对应的栅格地图并发布出去
PublishMap(scan_msg);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
std::cout << "\n转换一次地图用时: " << time_used_.count() << " 秒。" << std::endl;
}
PublishMap()
通过computemap计算Gmapping中栅格的值
gmapping认为ros的栅格地图数据只需要有3个值
- -1 代表栅格状态未知
- 0 代表栅格是空闲的,代表可通过区域
- 100 代表栅格是占用的,代表障碍物,不可通过
-
#define GMAPPING_UNKNOWN (-1) #define GMAPPING_FREE (0) #define GMAPPING_OCC (100) // 计算当前雷达数据对应的栅格地图并发布出去 void GMapping::PublishMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg) { // 地图的中点 Point center; center.x = (xmin_ + xmax_) / 2.0; center.y = (ymin_ + ymax_) / 2.0; // ScanMatcherMap为GMapping中存储地图的数据类型 ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_); // 使用当前雷达数据更新GMapping地图中栅格的值 ComputeMap(gmapping_map_, scan_msg); // 将gmapping_map_中的存储的栅格值 赋值到 ros的map中 for (int x = 0; x < gmapping_map_.getMapSizeX(); x++) { for (int y = 0; y < gmapping_map_.getMapSizeY(); y++) { IntPoint p(x, y); // 获取这点栅格的值,只有大于occ_thresh_时才认为是占用 double occ = gmapping_map_.cell(p); // 未知 if (occ < 0) map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_UNKNOWN; // 占用 else if (occ > occ_thresh_) // 默认0.25 map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC; // 空闲 else map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE; } } // 添加当前的时间戳 map_.header.stamp = ros::Time::now(); map_.header.frame_id = scan_msg->header.frame_id; // 发布map和map_metadata map_publisher_.publish(map_); map_publisher_metadata_.publish(map_.info); }
ComputeMap()
-
// 使用当前雷达数据更新GMapping地图中栅格的值 void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg) { line_lists_.clear(); hit_lists_.clear(); // lp为地图坐标系下的激光雷达坐标系的位姿 OrientedPoint lp(0, 0, 0.0); // 将位姿lp转换成地图坐标系下的位置 IntPoint p0 = map.world2map(lp); // 第一部分 // 地图的有效区域(地图坐标系) HierarchicalArray2D<PointAccumulator>::PointSet activeArea; // 通过激光雷达的数据,找出地图的有效区域 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++) { // 排除错误的激光点 double d = scan_msg->ranges[i]; if (d > max_range_ || d == 0.0 || !std::isfinite(d)) continue; if (d > max_use_range_) d = max_use_range_; // p1为激光雷达的数据点在地图坐标系下的坐标 Point phit = lp; phit.x += d * a_cos_[i]; phit.y += d * a_sin_[i]; IntPoint p1 = map.world2map(phit); // 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标 GridLineTraversalLine line; GridLineTraversal::gridLine(p0, p1, &line); // 将line保存起来以备后用 line_lists_.push_back(line); // 计算活动区域的大小 for (int i = 0; i < line.num_points - 1; i++) { activeArea.insert(map.storage().patchIndexes(line.points[i])); } // 如果d<m_usableRange则需要把击中点也算进去 说明这个值是好的。 // 同时如果d==max_use_range_那么说明这个值只用来进行标记空闲区域 不用来进行标记障碍物 if (d < max_use_range_) { IntPoint cp = map.storage().patchIndexes(p1); activeArea.insert(cp); hit_lists_.push_back(phit); } } // 为activeArea分配内存 map.storage().setActiveArea(activeArea, true); map.storage().allocActiveArea(); // 第二部分 // 在map上更新空闲点 for (auto line : line_lists_) { // 更新空闲位置 for (int k = 0; k < line.num_points - 1; k++) { // 未击中,就不记录击中的位置了,所以传入参数Point(0,0) map.cell(line.points[k]).update(false, Point(0, 0)); } } // 在map上添加hit点 for (auto hit : hit_lists_) { IntPoint p1 = map.world2map(hit); map.cell(p1).update(true, hit); } }
gridLine()
void GridLineTraversal::gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
int i, j;
int half;
IntPoint v;
gridLineCore(start, end, line);
if (start.x != line->points[0].x || start.y != line->points[0].y)
{
half = line->num_points / 2;
for (i = 0, j = line->num_points - 1; i < half; i++, j--)
{
v = line->points[i];
line->points[i] = line->points[j];
line->points[j] = v;
}
}
}
gridLineCore()
void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
int dx, dy; // 横纵坐标间距
int incr1, incr2; // 增量
int d; //
int x, y, xend, yend; // 直线增长的首末端点坐标
int xdirflag, ydirflag; // 横纵坐标增长方向
int cnt = 0; // 直线过点的点的序号
dx = abs(end.x - start.x);
dy = abs(end.y - start.y);
// 斜率绝对值小于等于1的情况,优先增加x
if (dy <= dx)
{
d = 2 * dy - dx; // 初始点P_m0值
incr1 = 2 * dy; // 情况(1)
incr2 = 2 * (dy - dx); // 情况(2)
// 将增长起点设置为横坐标小的点处,将 x的增长方向 设置为 向右侧增长
if (start.x > end.x)
{
// 起点横坐标比终点横坐标大,ydirflag = -1(负号可以理解为增长方向与直线始终点方向相反)
x = end.x;
y = end.y;
ydirflag = (-1);
xend = start.x; // 设置增长终点横坐标
}
else
{
x = start.x;
y = start.y;
ydirflag = 1;
xend = end.x;
}
//加入起点坐标
line->points.push_back(IntPoint(x, y));
cnt++;
// 向 右上 方向增长
if (((end.y - start.y) * ydirflag) > 0)
{
while (x < xend)
{
x++;
if (d < 0)
{
d += incr1;
}
else
{
y++;
d += incr2; // 纵坐标向正方向增长
}
line->points.push_back(IntPoint(x, y));
cnt++;
}
}
// 向 右下 方向增长
else
{
while (x < xend)
{
x++;
if (d < 0)
{
d += incr1;
}
else
{
y--;
d += incr2; // 纵坐标向负方向增长
}
line->points.push_back(IntPoint(x, y));
cnt++;
}
}
}
// 斜率绝对值大于1的情况,优先增加y
else
{
// dy > dx,当斜率k的绝对值|k|>1时,在y方向进行单位步进
d = 2 * dx - dy;
incr1 = 2 * dx;
incr2 = 2 * (dx - dy);
// 将增长起点设置为纵坐标小的点处,将 y的增长方向 设置为 向上侧增长
if (start.y > end.y)
{
y = end.y; // 取最小的纵坐标作为起点
x = end.x;
yend = start.y;
xdirflag = (-1);
}
else
{
y = start.y;
x = start.x;
yend = end.y;
xdirflag = 1;
}
// 添加起点
line->points.push_back(IntPoint(x, y));
cnt++;
// 向 上右 增长
if (((end.x - start.x) * xdirflag) > 0)
{
while (y < yend)
{
y++;
if (d < 0)
{
d += incr1;
}
else
{
x++;
d += incr2; // 横坐标向正方向增长
}
//添加新的点
line->points.push_back(IntPoint(x, y));
cnt++;
}
}
// 向 上左 增长
else
{
while (y < yend)
{
y++;
if (d < 0)
{
d += incr1;
}
else
{
x--;
d += incr2; //横坐标向负方向增长
}
line->points.push_back(IntPoint(x, y));
// 记录添加所有点的数目
cnt++;
}
}
}
line->num_points = cnt;
}
GMapping的地图相关的文件有3个,array.h,harray2d.h,map.h
地图占用值更新的实现
//PointAccumulator表示地图中一个cell(栅格)包括的内容
/*
acc: 栅格累计被击中位置
n: 栅格被击中次数
visits:栅格被访问的次数
*/
//PointAccumulator的一个对象,就是一个栅格,gmapping中其他类模板的cell就是这个
struct PointAccumulator
{
//float类型的point
typedef point<float> FloatPoint;
//构造函数
PointAccumulator() : acc(0, 0), n(0), visits(0) {}
PointAccumulator(int i) : acc(0, 0), n(0), visits(0) { assert(i == -1); }
//计算栅格被击中坐标累计值的平均值
inline Point mean() const { return 1. / n * Point(acc.x, acc.y); }
//返回该栅格被占用的概率,范围是 -1(没有访问过) 、[0,1]
inline operator double() const { return visits ? (double)n * 1 / (double)visits : -1; }
//更新该栅格成员变量
inline void update(bool value, const Point &p = Point(0, 0));
//该栅格被击中的位置累计,最后取累计值的均值
FloatPoint acc;
//n表示该栅格被击中的次数,visits表示该栅格被访问的次数
int n, visits;
};
//更新该栅格成员变量,value表示该栅格是否被击中,击中n++,未击中仅visits++;
void PointAccumulator::update(bool value, const Point &p)
{
if (value)
{
acc.x += static_cast<float>(p.x);
acc.y += static_cast<float>(p.y);
n++;
visits += 1;
}
else
visits++;
}