本文整理汇总了C++中costmap_2d::Costmap2D::getOriginY方法的典型用法代码示例。如果您正苦于以下问题：C++ Costmap2D::getOriginY方法的具体用法？C++ Costmap2D::getOriginY怎么用？C++ Costmap2D::getOriginY使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类costmap_2d::Costmap2D的用法示例。
在下文中一共展示了Costmap2D::getOriginY方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: setPathCells
//update what map cells are considered path based on the global_plan
void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
queue<MapCell*> path_dist_queue;
queue<MapCell*> goal_dist_queue;
for(unsigned int i = 0; i < global_plan.size(); ++i){
double g_x = global_plan[i].pose.position.x;
double g_y = global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if(costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION){
MapCell& current = getCell(map_x, map_y);
current.path_dist = 0.0;
current.path_mark = true;
path_dist_queue.push(¤t);
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else{
if(started_path)
break;
}
}
if(local_goal_x >= 0 && local_goal_y >= 0){
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
}
//compute our distances
computePathDistance(path_dist_queue, costmap);
computeGoalDistance(goal_dist_queue, costmap);
}