void upTo(costmap_2d::Costmap2D &gradient_){
for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
//if(gradient_.getCost(varX, varY) == 0)
gradient_.setCost(varX, varY, DEF);
}
}
}
bool CSUROGlobalPlanner::isCoordInMap(const costmap_2d::Costmap2D &costmap, const float& x, const float& y)
{
int cellI, cellJ;
original_costmap_.worldToMapEnforceBounds(x, y, cellI, cellJ);
if(cellI < costmap.getSizeInCellsX() && cellI >= 0 && cellJ < costmap.getSizeInCellsY() && cellJ >= 0)
return true;
return false;
}
bool inMap(costmap_2d::Costmap2D &gradient_, int i, int j)
{
if(i < gradient_.getSizeInCellsX() && i >= 0 && j < gradient_.getSizeInCellsY() && j >= 0)
{
return true;
}
else
{
return false;
}
}
//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::setTargetCells (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size())
{
ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (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)
{
MapCell& current = getCell (map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
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 InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
boost::unique_lock < boost::shared_mutex > lock(*access_);
if (!enabled_)
return;
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max( 0, min_i );
min_j = std::max( 0, min_j );
max_i = std::min( int( size_x ), max_i );
max_j = std::min( int( size_y ), max_j );
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
enqueue(master_array, index, i, j, i, j);
}
}
}
while (!inflation_queue_.empty())
{
//get the highest priority cell and pop it off the priority queue
const CellData& current_cell = inflation_queue_.top();
unsigned int index = current_cell.index_;
unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;
//attempt to put the neighbors of the current cell onto the queue
if (mx > 0)
enqueue(master_array, index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(master_array, index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(master_array, index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(master_array, index + size_x, mx, my + 1, sx, sy);
//remove the current cell from the priority queue
inflation_queue_.pop();
}
}
bool transformGlobalPlan(
const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const tf::Stamped<tf::Pose>& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try {
if (!global_plan.size() > 0) {
ROS_ERROR("Received plan with zero length");
return false;
}
// get plan_to_global_transform from plan frame to global_frame
tf::StampedTransform plan_to_global_transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(plan_to_global_transform * tf_pose);
tf_pose.stamp_ = plan_to_global_transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (!enabled_ || (cell_inflation_radius_ == 0))
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
unsigned int index = cell.index_;
// ignore if already visited
if (seen_[index])
{
continue;
}
seen_[index] = true;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);
// attempt to put the neighbors of the current cell onto the inflation list
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
inflation_cells_.clear();
}
