//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_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)
{
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else
{
if (started_path)
{
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path)
{
ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
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.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
}
computeTargetDistance (path_dist_queue, costmap);
}
//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);
}
void fillPlan(costmap_2d::Costmap2D gradient_, std::vector<geometry_msgs::PoseStamped>& plan, const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, int startI, int startJ, int goalI, int goalJ)
{
int iPoint = startI;
int jPoint = startJ;
plan.push_back(start);
double x,y;
int counter = 0;
geometry_msgs::PoseStamped before = start;
geometry_msgs::PoseStamped point;
tf::Quaternion point_quat;
while(isEnd(gradient_, iPoint, jPoint, goalI, goalJ) == false)
{
counter++;
getMin(gradient_, iPoint, jPoint);
gradient_.mapToWorld (iPoint, jPoint, x, y);
point.pose.position.x = x;
point.pose.position.y = y;
point_quat = tf::createQuaternionFromYaw(atan2 (y, x));
point.pose.orientation.x = point_quat.x();
point.pose.orientation.y = point_quat.y();
point.pose.orientation.z = point_quat.z();
point.pose.orientation.w = point_quat.w();
if(counter%2 == 0)
plan.push_back(point);
before = point;
}
plan.push_back(goal);
}