|
_);
}
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
x0 = std::max(0, x0);
xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
if (xn < x0 || yn < y0)
return;
costmap_.resetMap(x0, y0, xn, yn);
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
//cost 值的更新，每个plugin都实现这个函数,是一份costmap在不同的plugin下的更新，costmap是一样的，cost值是叠加上去的
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
}
}
}
}
namespace costmap_2d
{
class StaticLayer : public CostmapLayer
{
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
}
}
namespace costmap_2d
{
class ObstacleLayer : public CostmapLayer
{
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void activate();
void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
https://zhuanlan.zhihu.com/p/21738718?refer=robotics-learning const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_y);
}
}
怎么计算每个costmap中的单元格的cost代码还没有研究
Tips
开启ros debug level的模式
栅格地图理论编程
|