void VirtualWallLayer::onWallMessageCallback(const WallPtr& msg) {
WallInfo wi;
if(worldToMap(msg->from.x, msg->from.y, wi.x1, wi.y1) && worldToMap(msg->to.x, msg->to.y, wi.x2, wi.y2)) {
walls[msg->identifier] = wi;
updateWalls();
} else {
walls.erase(msg->identifier);
}
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
//get the cell coordinates of the center point of the window
unsigned int mx, my;
if (!worldToMap(wx, wy, mx, my))
return;
//compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
//scale the window based on the bounds of the costmap
start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
//check for legality just in case
if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
return;
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for (unsigned int j = map_sy; j <= map_ey; ++j)
{
for (unsigned int i = map_sx; i <= map_ex; ++i)
{
//if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if (*current != LETHAL_OBSTACLE)
{
if (clear_no_info || *current != NO_INFORMATION)
{
*current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
bool EpicNavigationNodeOMPL::srvRemoveGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (occupancy_grid == nullptr) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvRemoveGoals]: Occupancy grid was not initialized.");
res.success = false;
return false;
}
if (req.goals.size() != 1) {
ROS_ERROR("Warning[EpicNavigationNodeOMPL::srvRemoveGoals]: Invalid number of goal locations.");
res.success = false;
return false;
}
float x = 0.0f;
float y = 0.0f;
worldToMap(req.goals[0].pose.position.x, req.goals[0].pose.position.y, x, y);
// If the goal location is not a goal, then we can just return without changing anything.
if (!isCellGoal((unsigned int)(x + 0.5f), (unsigned int)(y + 0.5f))) {
return ;
}
occupancy_grid[(unsigned int)(y + 0.5f) * width + (unsigned int)(x + 0.5f)] = EPIC_CELL_TYPE_FREE;
goal_assigned = false;
// Try to create the planner algorithm now.
initAlg();
res.success = true;
return true;
}
bool EpicNavigationNodeHarmonic::srvRemoveGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvRemoveGoals]: Algorithm was not initialized.");
res.success = false;
return false;
}
std::vector<unsigned int> v;
std::vector<unsigned int> types;
// Note: Removing goals turns them into free space. Recall, however, that goals can
// only be added on free space (above).
for (unsigned int i = 0; i < req.goals.size(); i++) {
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(req.goals[i].pose.position.x, req.goals[i].pose.position.y, x, y)) {
continue;
}
v.push_back((unsigned int) x);
v.push_back((unsigned int) y);
types.push_back(EPIC_CELL_TYPE_FREE);
}
setCells(v, types);
v.clear();
types.clear();
res.success = true;
return true;
}
bool GridMap2D::isOccupiedAt(double wx, double wy) const{
unsigned mx, my;
if (worldToMap(wx, wy, mx, my))
return isOccupiedAtCell(mx, my);
else
return true;
}
bool EpicNavigationNodeOMPL::srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res)
{
// First, determine and assign the starting location.
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(req.start.pose.position.x, req.start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvComputePath]: Could not convert start to floating point map location.");
}
start_location.first = x;
start_location.second = y;
start_assigned = true;
// Try to create the planner algorithm now that we have an initial starting state.
initAlg();
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvComputePath]: Algorithm was not initialized.");
return false;
}
unsigned int k = 0;
float *raw_path = nullptr;
if ((bool)ompl_planner_status) {
// TODO: If an approximate or exact solution was found, then populate raw_path. Otherwise, leave it empty.
//k = ???
//raw_path = new float[2 * k];
//for (...
// ompl_planner.get_path...???
}
res.path.header.frame_id = req.start.header.frame_id;
res.path.header.stamp = req.start.header.stamp;
res.path.poses.resize(k);
res.path.poses[0] = req.start;
for (unsigned int i = 1; i < k; i++) {
x = raw_path[2 * i + 0];
y = raw_path[2 * i + 1];
float path_theta = std::atan2(y - raw_path[2 * (i - 1) + 1], x - raw_path[2 * (i - 1) + 0]);
float path_x = 0.0f;
float path_y = 0.0f;
mapToWorld(x, y, path_x, path_y);
res.path.poses[i] = req.start;
res.path.poses[i].pose.position.x = path_x;
res.path.poses[i].pose.position.y = path_y;
res.path.poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(path_theta);
}
delete [] raw_path;
raw_path = nullptr;
return true;
}
uchar GridMap2D::binaryMapAt(double wx, double wy) const{
unsigned mx, my;
if (worldToMap(wx, wy, mx, my))
return m_binaryMap.at<uchar>(mx, my);
else
return 0;
}
float GridMap2D::distanceMapAt(double wx, double wy) const{
unsigned mx, my;
if (worldToMap(wx, wy, mx, my))
return m_distMap.at<float>(mx, my);
else
return -1.0f;
}
void Costmap2D::resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
priority_queue<CellData>& inflation_queue, bool clear){
//get the cell coordinates of the center point of the window
unsigned int mx, my;
if(!worldToMap(wx, wy, mx, my))
return;
//compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
//scale the window based on the bounds of the costmap
start_x = max(origin_x_, start_x);
start_y = max(origin_y_, start_y);
end_x = min(origin_x_ + getSizeInMetersX(), end_x);
end_y = min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
//check for legality just in case
if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)){
ROS_ERROR("Bounds not legal for reset window. Doing nothing.");
return;
}
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for(unsigned int j = map_sy; j <= map_ey; ++j){
for(unsigned int i = map_sx; i <= map_ex; ++i){
//if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if(*current == LETHAL_OBSTACLE)
enqueue(index, i, j, i, j, inflation_queue);
else if(clear && *current != NO_INFORMATION)
*current = FREE_SPACE;
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
float DynamicEDTOctomap::getDistance(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
return data[x][y][z].dist*treeResolution;
} else {
return distanceValue_Error;
}
}
int DynamicEDTOctomap::getSquaredDistanceInCells(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
if(x>=0 && x<sizeX && y>=0 && y<sizeY && z>=0 && z<sizeZ){
return data[x][y][z].sqdist;
} else {
return distanceInCellsValue_Error;
}
}
float cartman_mpd_t::get_level(float x, float y)
{
float zdone = 0.0f;
if (x < 0.0f || x >= info.edgex) return zdone;
if (y < 0.0f || y >= info.edgey) return zdone;
int mapx, mapy;
worldToMap(x, y, mapx, mapy);
return get_level(mapx, mapy);
}
void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
double start_point_x = wx - w_size_x / 2;
double start_point_y = wy - w_size_y / 2;
double end_point_x = start_point_x + w_size_x;
double end_point_y = start_point_y + w_size_y;
//check start bounds
start_point_x = max(origin_x_, start_point_x);
start_point_y = max(origin_y_, start_point_y);
//check end bounds
end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
unsigned int start_x, start_y, end_x, end_y;
//check for legality just in case
if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y))
return;
ROS_ASSERT(end_x >= start_x && end_y >= start_y);
unsigned int cell_size_x = end_x - start_x;
unsigned int cell_size_y = end_y - start_y;
//we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
//now we'll reset the costmap to the static map
memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
//clean up
delete[] local_map;
}
void DynamicEDTOctomap::getDistanceAndClosestObstacle_unsafe(const octomap::point3d& p, float &distance, octomap::point3d& closestObstacle) const {
int x,y,z;
worldToMap(p, x, y, z);
dataCell c= data[x][y][z];
distance = c.dist*treeResolution;
if(c.obstX != invalidObstData){
mapToWorld(c.obstX, c.obstY, c.obstZ, closestObstacle);
} else {
//If we are at maxDist, it can very well be that there is no valid closest obstacle data for this cell, this is not an error.
}
}
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return;
if (!layered_costmap_->isRolling())
{
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
if (!use_maximum_)
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
else
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
else
{
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
tf::StampedTransform transform;
try
{
tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
// Copy map data given proper transformations
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
{
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf::Point p(wx, wy, 0);
p = transform(p);
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));
else
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
}
}
}
}
}
void Costmap2D::reshapeStaticMap(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data){
int m_ox, m_oy;
worldToMapNoBounds(win_origin_x, win_origin_y, m_ox, m_oy);
//compute the bounds for the size of our new map
int bl_x = std::min(m_ox, 0);
int bl_y = std::min(m_oy, 0);
int ur_x = std::max(m_ox + data_size_x, size_x_);
int ur_y = std::max(m_oy + data_size_y, size_y_);
//create a temporary map to hold our static data and copy the old static map into it
unsigned char* static_map_copy = new unsigned char[size_x_ * size_y_];
memcpy(static_map_copy, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//delete our old maps... the user will lose any
//cost information not stored in the static map when reshaping a map
deleteMaps();
//update the origin and sizes, and cache data we'll need
double old_origin_x = origin_x_;
double old_origin_y = origin_y_;
unsigned int old_size_x = size_x_;
unsigned int old_size_y = size_y_;
size_x_ = ur_x - bl_x;
size_y_ = ur_y - bl_y;
origin_x_ = std::min(origin_x_, win_origin_x);
origin_y_ = std::min(origin_y_, win_origin_y);
//initialize our various maps
initMaps(size_x_, size_y_);
//reset our maps to be full of unknown space if appropriate
resetMaps();
//now, copy the old static map into the new costmap
unsigned int start_x, start_y;
worldToMap(old_origin_x, old_origin_y, start_x, start_y);
copyMapRegion(static_map_copy, 0, 0, old_size_x, costmap_, start_x, start_y, size_x_, old_size_x, old_size_y);
delete[] static_map_copy;
//now we want to update the map with the new static map data
replaceStaticMapWindow(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data);
}
void GridLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
double mark_x = origin_x + cos(origin_yaw), mark_y = origin_y + sin(origin_yaw);
unsigned int mx;
unsigned int my;
if(worldToMap(mark_x, mark_y, mx, my)){
setCost(mx, my, LETHAL_OBSTACLE);
}
*min_x = std::min(*min_x, mark_x);
*min_y = std::min(*min_y, mark_y);
*max_x = std::max(*max_x, mark_x);
*max_y = std::max(*max_y, mark_y);
}
bool EpicNavigationNodeHarmonic::srvAddGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvAddGoals]: Algorithm was not initialized.");
res.success = false;
return false;
}
std::vector<unsigned int> v;
std::vector<unsigned int> types;
for (unsigned int i = 0; i < req.goals.size(); i++) {
float x = 0.0f;
float y = 0.0f;
worldToMap(req.goals[i].pose.position.x, req.goals[i].pose.position.y, x, y);
// If the goal location is an obstacle, then do not let it add a goal here.
if (isCellObstacle((unsigned int)(x + 0.5f), (unsigned int)(y + 0.5f))) {
continue;
}
v.push_back((unsigned int) x);
v.push_back((unsigned int) y);
types.push_back(EPIC_CELL_TYPE_GOAL);
}
if (v.size() == 0) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvAddGoals]: Attempted to add goal(s) inside obstacles. No goals added.");
res.success = false;
return false;
}
setCells(v, types);
v.clear();
types.clear();
res.success = true;
return true;
}
void Costmap2D::updateObstacles(const vector<Observation>& observations, priority_queue<CellData>& inflation_queue){
//place the new obstacles into a priority queue... each with a priority of zero to begin with
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
const sensor_msgs::PointCloud& cloud =obs.cloud_;
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
for(unsigned int i = 0; i < cloud.points.size(); ++i){
//if the obstacle is too high or too far away from the robot we won't add it
if(cloud.points[i].z > max_obstacle_height_){
ROS_DEBUG("The point is too high");
continue;
}
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
+ (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
+ (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
//if the point is far enough away... we won't consider it
if(sq_dist >= sq_obstacle_range){
ROS_DEBUG("The point is too far away");
continue;
}
//now we need to compute the map coordinates for the observation
unsigned int mx, my;
if(!worldToMap(cloud.points[i].x, cloud.points[i].y, mx, my)){
ROS_DEBUG("Computing map coords failed");
continue;
}
unsigned int index = getIndex(mx, my);
//push the relevant cell index back onto the inflation queue
enqueue(index, mx, my, mx, my, inflation_queue);
}
}
}
bool EpicNavigationNodeOMPL::srvAddGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (occupancy_grid == nullptr) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Occupancy grid was not initialized.");
res.success = false;
return false;
}
if (req.goals.size() != 1) {
ROS_ERROR("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Invalid number of goal locations.");
res.success = false;
return false;
}
float x = 0.0f;
float y = 0.0f;
worldToMap(req.goals[0].pose.position.x, req.goals[0].pose.position.y, x, y);
// If the goal location is an obstacle, then do not let it add a goal here.
if (isCellObstacle((unsigned int)(x + 0.5f), (unsigned int)(y + 0.5f))) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Attempted to add a goal at an obstacle.");
res.success = false;
return false;
}
goal_location.first = x;
goal_location.second = y;
goal_assigned = true;
occupancy_grid[(unsigned int)(y + 0.5f) * width + (unsigned int)(x + 0.5f)] = EPIC_CELL_TYPE_OBSTACLE;
// Try to create the planner algorithm now.
initAlg();
res.success = true;
return true;
}
void BallPickerLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
if (layered_costmap_->isRolling())
updateOrigin(origin_x - getSizeInMetersX() / 2, origin_y - getSizeInMetersY() / 2);
double mark_x, mark_y;
unsigned int mx, my;
unsigned int index;
if (clear_flag)
{
*min_x = *min_x - getSizeInMetersX()/2;
*min_y = *min_y - getSizeInMetersY()/2;
*max_x = *max_x + getSizeInMetersX()/2;
*max_y = *max_y + getSizeInMetersY()/2;
}
for (std::vector<ball_picker::PointOfInterest>::iterator iter = obstacle_buffer.begin() ; iter != obstacle_buffer.end(); iter++)
{
mark_x = iter->x;
mark_y = iter->y;
if(worldToMap(mark_x, mark_y, mx, my))
{
index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
}
*min_x = std::min(*min_x, mark_x);
*min_y = std::min(*min_y, mark_y);
*max_x = std::max(*max_x, mark_x);
*max_y = std::max(*max_y, mark_y);
}
}
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value) {
//we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for(unsigned int i = 0; i < polygon.size(); ++i){
MapLocation loc;
if(!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)){
ROS_DEBUG("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}
std::vector<MapLocation> polygon_cells;
//get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);
//set the cost of those cells
for(unsigned int i = 0; i < polygon_cells.size(); ++i){
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
}
return true;
}
bool GridMap2D::inMapBounds(double wx, double wy) const{
unsigned mx, my;
return worldToMap(wx,wy,mx,my);
}
int DynamicEDTOctomap::getSquaredDistanceInCells_unsafe(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
return data[x][y][z].sqdist;
}
bool EpicNavigationNodeHarmonic::srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res)
{
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvComputePath]: Algorithm was not initialized.");
return false;
}
if (gpu) {
if (harmonic_get_potential_values_gpu(&harmonic) != EPIC_SUCCESS) {
ROS_WARN("Error[EpicNavigationNodeHarmonic::srvComputePath]: Failed to get the potential values from the GPU.");
return false;
}
}
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(req.start.pose.position.x, req.start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvComputePath]: Could not convert start to floating point map location.");
}
unsigned int k = 0;
float *raw_path = nullptr;
int result = harmonic_compute_path_2d_cpu(&harmonic, x, y, req.step_size, req.precision, req.max_length, k, raw_path);
if (result != EPIC_SUCCESS) {
ROS_ERROR("Error[EpicNavigationNodeHarmonic::srvComputePath]: Failed to compute the path.");
if (raw_path != nullptr) {
delete [] raw_path;
}
return false;
}
res.path.header.frame_id = req.start.header.frame_id;
res.path.header.stamp = req.start.header.stamp;
res.path.poses.resize(k);
res.path.poses[0] = req.start;
for (unsigned int i = 1; i < k; i++) {
x = raw_path[2 * i + 0];
y = raw_path[2 * i + 1];
float path_theta = std::atan2(y - raw_path[2 * (i - 1) + 1], x - raw_path[2 * (i - 1) + 0]);
float path_x = 0.0f;
float path_y = 0.0f;
mapToWorld(x, y, path_x, path_y);
res.path.poses[i] = req.start;
res.path.poses[i].pose.position.x = path_x;
res.path.poses[i].pose.position.y = path_y;
res.path.poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(path_theta);
}
delete [] raw_path;
raw_path = nullptr;
return true;
}
float DynamicEDTOctomap::getDistance_unsafe(const octomap::point3d& p) const {
int x,y,z;
worldToMap(p, x, y, z);
return data[x][y][z].dist*treeResolution;
}
void Costmap2D::replaceStaticMapWindow(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y,
const std::vector<unsigned char>& static_data){
unsigned int start_x, start_y;
if(!worldToMap(win_origin_x, win_origin_y, start_x, start_y) || (start_x + data_size_x) > size_x_ || (start_y + data_size_y) > size_y_){
ROS_ERROR("You must call replaceStaticMapWindow with a window origin and size that is contained within the map");
return;
}
//we need to compute the region of the costmap that could change from inflation of new obstacles
unsigned int max_inflation_change = 2 * cell_inflation_radius_;
//make sure that we don't go out of map bounds
unsigned int copy_sx = std::min(std::max(0, (int)start_x - (int)max_inflation_change), (int)size_x_);
unsigned int copy_sy = std::min(std::max(0, (int)start_y - (int)max_inflation_change), (int)size_x_);
unsigned int copy_ex = std::max(std::min((int)size_x_, (int)start_x + (int)data_size_x + (int)max_inflation_change), 0);
unsigned int copy_ey = std::max(std::min((int)size_y_, (int)start_y + (int)data_size_y + (int)max_inflation_change), 0);
unsigned int copy_size_x = copy_ex - copy_sx;
unsigned int copy_size_y = copy_ey - copy_sy;
//now... we have to compute the window
double ll_x, ll_y, ur_x, ur_y;
mapToWorld(copy_sx, copy_sy, ll_x, ll_y);
mapToWorld(copy_ex, copy_ey, ur_x, ur_y);
double mid_x = (ll_x + ur_x) / 2;
double mid_y = (ll_y + ur_y) / 2;
double size_x = ur_x - ll_x;
double size_y = ur_y - ll_y;
//finally... we'll clear all non-lethal costs in the area that could be affected by the map update
//we'll reinflate after the map update is complete
clearNonLethal(mid_x, mid_y, size_x, size_y);
//copy static data into the costmap
unsigned int start_index = start_y * size_x_ + start_x;
unsigned char* costmap_index = costmap_ + start_index;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
for(unsigned int i = 0; i < data_size_y; ++i){
for(unsigned int j = 0; j < data_size_x; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
++costmap_index;
++static_data_index;
}
costmap_index += size_x_ - data_size_x;
}
//now, we're ready to reinflate obstacles in the window that has been updated
//we won't clear all non-lethal obstacles first because the static map update
//may have included non-lethal costs
reinflateWindow(mid_x, mid_y, size_x, size_y, false);
//we also want to keep a copy of the current costmap as the static map... we'll only need to write the region that has changed
copyMapRegion(costmap_, copy_sx, copy_sy, size_x_, static_map_, copy_sx, copy_sy, size_x_, copy_size_x, copy_size_y);
}
void Costmap2D::raytraceFreespace(const Observation& clearing_observation){
//create the functor that we'll use to clear cells from the costmap
ClearCell clearer(costmap_);
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
sensor_msgs::PointCloud cloud = clearing_observation.cloud_;
//get the map coordinates of the origin of the sensor
unsigned int x0, y0;
if(!worldToMap(ox, oy, x0, y0)){
ROS_WARN("The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy);
return;
}
//we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
//for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
for(unsigned int i = 0; i < cloud.points.size(); ++i){
double wx = cloud.points[i].x;
double wy = cloud.points[i].y;
//now we also need to make sure that the enpoint we're raytracing
//to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
//the minimum value to raytrace from is the origin
if(wx < origin_x_){
double t = (origin_x_ - ox) / a;
wx = origin_x_;
wy = oy + b * t;
}
if(wy < origin_y_){
double t = (origin_y_ - oy) / b;
wx = ox + a * t;
wy = origin_y_;
}
//the maximum value to raytrace to is the end of the map
if(wx > map_end_x){
double t = (map_end_x - ox) / a;
wx = map_end_x;
wy = oy + b * t;
}
if(wy > map_end_y){
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y;
}
//now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
//check for legality just in case
if(!worldToMap(wx, wy, x1, y1))
continue;
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
//and finally... we can execute our trace to clear obstacles along that line
raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range);
}
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN(
"The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
timespec time1, time2;
/* take current time here */
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
std::cout<<"time to generate best global path time = " << (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 << " microseconds" <<"\n";
//data<<"["<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 ;
// add orientations if needed
orientation_filter_->processPath(start, plan);
float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.end(); ++it) {
std::cout<<"路径位姿点：("<<(*it).pose.position.x<<","<<(*it).pose.position.y<<")"<<"\n";
path_length += hypot( (*it).pose.position.x - last_pose.pose.position.x,
(*it).pose.position.y - last_pose.pose.position.y );
last_pose = *it;
}
std::cout <<"------The global path length: "<< path_length<< " meters------"<<"\n";
//data <<"["<< path_length<< ",";
//data<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
std::cout <<"["<< path_length<< ",";
std::cout<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
