boolCostmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsignedchar cost_value) { // we assume the polygon is given in the global_frame... we need to transform it to map coordinates std::vector<MapLocation> map_polygon; for (unsignedint i = 0; i < polygon.size(); ++i) { MapLocation loc; // 先将世界系下的多边形顶点转换到地图坐标系，并存放进map_polygon数组 if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) { // ("Polygon lies outside map bounds, so we can't fill it"); returnfalse; } map_polygon.push_back(loc); } std::vector<MapLocation> polygon_cells;
// 通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在 polygon_cells // get the cells that fill the polygon convexFillCells(map_polygon, polygon_cells);
// set the cost of those cells // 通过循环对多边形边缘及内部各cell的cost赋值 for (unsignedint i = 0; i < polygon_cells.size(); ++i) { unsignedint index = getIndex(polygon_cells[i].x, polygon_cells[i].y); costmap_[index] = cost_value; } returntrue; }
voidCostmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells) { // we need a minimum polygon of a triangle if (polygon.size() < 3) return;
// first get the cells that make up the outline of the polygon //首先获得轮廓点之间连线的列表，存放在polygon_cells中 polygonOutlineCells(polygon, polygon_cells);
// quick bubble sort to sort points by x MapLocation swap; unsignedint i = 0; // 对边上的cell点的x做排序，使其按x坐标升序排列 while (i < polygon_cells.size() - 1) { if (polygon_cells[i].x > polygon_cells[i + 1].x) { swap = polygon_cells[i]; polygon_cells[i] = polygon_cells[i + 1]; polygon_cells[i + 1] = swap;
i += 2; while (i < polygon_cells.size() && polygon_cells[i].x == x) { if (polygon_cells[i].y < min_pt.y) min_pt = polygon_cells[i]; elseif (polygon_cells[i].y > max_pt.y) max_pt = polygon_cells[i]; ++i; }
MapLocation pt; // loop though cells in the column for (unsignedint y = min_pt.y; y < max_pt.y; ++y) { pt.x = x; pt.y = y; polygon_cells.push_back(pt); } } }
voidCostmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells) { PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); for (unsignedint i = 0; i < polygon.size() - 1; ++i) { raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); } if (!polygon.empty()) { unsignedint last_index = polygon.size() - 1; // we also need to close the polygon by going from the last point to the first raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y); } }
int offset_dx = sign(dx); int offset_dy = sign(dy) * size_x_;
unsignedint offset = y0 * size_x_ + x0;
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line double dist = hypot(dx, dy); double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant if (abs_dx >= abs_dy) { int error_y = abs_dx / 2; bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsignedint)(scale * abs_dx)); return; } // otherwise y is dominant int error_x = abs_dy / 2; bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsignedint)(scale * abs_dy)); }
for (int x = 0; x < map_mat.cols; x++) for (int y = 0; y < map_mat.rows; y++) { map_mat.at<uint8_t>(map_mat.rows -1 - y, x) = 255 - costmap_->getCharMap()[ y * map_mat.cols + x]; }
Blocks until this goal finishes. 可以指定阻塞的时间，如果是0，则无线阻塞。
SimpleClientGoalState getState() const
Get the state information for this goal. Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
如果在服务端的回调函数里没有对goal的状态进行设置，会有下面报警
Your executeCallback did not set the goal to a terminal status. This is a bug in your ActionServer implementation. Fix your code! For now, the ActionServer will set this goal to aborted 因为没有设置goal的最终状态，比如使用setSucceeded和setAborted设置状态.
cancelGoal
cancelGoal()之后，getState()的状态还是ACTIVE
如果没有goal处于ACTIVE，不要使用cancelGoal，会报错但是不会让进程退出：
两个回调函数
actionlib::SimpleActionServer的成员函数
1 2 3 4 5
// Allows users to register a callback to be invoked when a new goal is available. voidregisterGoalCallback(boost::function< void()> cb) // Allows users to register a callback to be invoked when a new preempt request is available. voidregisterPreemptCallback(boost::function< void()> cb)
有时候我在move_base设置了setAborted之后，会出现报警 To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 4 。 这是因为多次调用了 setAborted，state 4 其实就是状态 Aborted。
此时如果再发送goal，就会出现报警 Your executeCallback did not set the goal to a terminalstatus. This is a bug in your ActionServer implementation. Fix your code! For now, the ActionServer will set this goal to aborted ，这打乱了 Action 的状态机，必须避免这个报警。
启动simple_world.launch，然后在Gazebo里添加一个圆柱体，保存覆盖simple.world文件。再次启动时，报错 SpawnModel: Failure - model name sunny already exist. [Spawn service failed. Exiting. 和 [spawn_urdf-6] process has died
Trims submaps that have less than min_covered_cells_count cells not overlapped by at least fresh_submaps_count submaps. 也就是说一些子图如果只有min_covered_cells_count个栅格没有被fresh_submaps_count个子图覆盖，那么这些子图被裁剪掉。
1 2 3 4 5 6 7 8 9 10
voidPoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) { // C++11 does not allow us to move a unique_ptr into a lambda. PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); trimmers_.emplace_back(trimmer_ptr); return WorkItem::Result::kDoNotRunOptimization; }); }
