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(); }