bool ChooseAccessibleBalls::canMove(float x, float y){ return true; // ROS_INFO("enter canMove"); // Get a copy of the current costmap to test. (threadsafe) costmap_2d::Costmap2D costmap; // ROS_INFO("aaa"); if(costmap_ros != NULL) costmap_ros->getCostmapCopy( costmap ); // ROS_INFO("bbb"); // Coordinate transform. unsigned int cell_x, cell_y; if( !costmap.worldToMap( x, y, cell_x, cell_y )){ return false; } double cost = double( costmap.getCost( cell_x, cell_y )); if(cost <= 127){ return true; } else{ return false; } }
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master_array = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = j * span + min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] == NO_INFORMATION){ it++; continue; } unsigned char old_cost = master_array[it]; if (old_cost == NO_INFORMATION) master_array[it] = costmap_[it]; else { int sum = old_cost + costmap_[it]; if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; else master_array[it] = sum; } it++; } } }
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master_array = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = j * span + min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] == NO_INFORMATION){ it++; continue; } unsigned char old_cost = master_array[it]; if (old_cost == NO_INFORMATION || old_cost < costmap_[it]) master_array[it] = costmap_[it]; it++; } } }
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; }
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned int mx; unsigned int my; if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){ master_grid.setCost(mx, my, LETHAL_OBSTACLE); } }
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; } }
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))); } } } } }
//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); }
double getCost(float x, float y){ // Get a copy of the current costmap to test. (threadsafe) costmap_2d::Costmap2D costmap; costmap_ros->getCostmapCopy( costmap ); // Coordinate transform. unsigned int cell_x, cell_y; if( !costmap.worldToMap( x, y, cell_x, cell_y )){ return -1.0; } double cost = double( costmap.getCost( cell_x, cell_y )); // ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost); return cost; }
inline bool MapGrid::updatePathCell (MapCell* current_cell, MapCell* check_cell, const costmap_2d::Costmap2D& costmap) { //if the cell is an obstacle set the max path distance unsigned char cost = costmap.getCost (check_cell->cx, check_cell->cy); if (! getCell (check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)) { check_cell->target_dist = obstacleCosts(); return false; } double new_target_dist = current_cell->target_dist + 1; if (new_target_dist < check_cell->target_dist) { check_cell->target_dist = new_target_dist; } return true; }
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = span*j+min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] != NO_INFORMATION) master[it] = costmap_[it]; it++; } } }
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned int mx; unsigned int my; if(pode_marcar){ if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){ for(int i = -1; i < 1; i++) master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE); } pode_marcar = false; } // ROS_INFO("Hi!"); }
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; const unsigned char* master_array = master_grid.getCharMap(); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { if (clear_flag) { master_grid.setCost(i, j, FREE_SPACE); } else { int index = getIndex(i, j); if (costmap_[index] == NO_INFORMATION) continue; unsigned char old_cost = master_array[index]; if (old_cost == NO_INFORMATION || old_cost < costmap_[index]) master_grid.setCost(i, j, costmap_[index]); } } } for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++) costmap_[i] == NO_INFORMATION; if (!obstacle_buffer.empty()) { ROS_INFO("Costmap updated with ball obstacles."); obstacle_buffer.clear(); std_srvs::Empty emptysrv; confirm_update_client.call(emptysrv); } }
bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost) { if(inMap(gradient_, i, j) == false){ //ROS_INFO("salgo por no estar en mapa"); return false; } if(original_.getCost( i, j) != 0 ){ //ROS_INFO("salgo por ser obstaculo"); return false; } if(gradient_.getCost( i, j) <= cost){ //ROS_INFO("salgo por ser igual o menor"); return false; } return true; }
bool canMove(float x, float y){ // Get a copy of the current costmap to test. (threadsafe) costmap_2d::Costmap2D costmap; costmap_ros->getCostmapCopy( costmap ); // Coordinate transform. unsigned int cell_x, cell_y; if( !costmap.worldToMap( x, y, cell_x, cell_y )){ // res.cost = -1.0; return false; } double cost = double( costmap.getCost( cell_x, cell_y )); ROS_INFO("cost = %f", cost); if(cost <= 1){ return true; } else{ return false; } //ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost); }
void VirtualWallLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = getIndex(i, j); if (costmap_[index] == NO_INFORMATION) continue; master_grid.setCost(i, j, costmap_[index]); } } }
//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); }
void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ) { gradient_.setCost(i, j, value); //ROS_INFO("(%d, %d) <- %d", i, j, value); value = value + 5; if(value>254) value=254; if(go(gradient_, original_, i-1,j, value)) spread(gradient_, original_, value, i-1, j, startCellI, startCellJ); if(go(gradient_, original_, i,j-1, value)) spread(gradient_, original_, value, i, j-1, startCellI, startCellJ); if(go(gradient_, original_, i+1,j, value)) spread(gradient_, original_, value, i+1, j, startCellI, startCellJ); if(go(gradient_, original_, i,j+1, value)) spread(gradient_, original_, value, i, j+1, startCellI, startCellJ); }
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); }
//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); }
/** * get the cellsof a footprint at a given position */ std::vector<upo_navigation::Position2DInt> FootprintHelper::getFootprintCells( Eigen::Vector3f pos, std::vector<geometry_msgs::Point> footprint_spec, const costmap_2d::Costmap2D& costmap, bool fill){ double x_i = pos[0]; double y_i = pos[1]; double theta_i = pos[2]; std::vector<upo_navigation::Position2DInt> footprint_cells; //std::vector<geometry_msgs::Point> footprint_spec_ = costmap_ros_->getRobotFootprint(); //if we have no footprint... do nothing if (footprint_spec.size() <= 1) { unsigned int mx, my; if (costmap.worldToMap(x_i, y_i, mx, my)) { upo_navigation::Position2DInt center; center.x = mx; center.y = my; footprint_cells.push_back(center); } return footprint_cells; } //pre-compute cos and sin values double cos_th = cos(theta_i); double sin_th = sin(theta_i); double new_x, new_y; unsigned int x0, y0, x1, y1; unsigned int last_index = footprint_spec.size() - 1; for (unsigned int i = 0; i < last_index; ++i) { //find the cell coordinates of the first segment point new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); if(!costmap.worldToMap(new_x, new_y, x0, y0)) { return footprint_cells; } //find the cell coordinates of the second segment point new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th); new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th); if (!costmap.worldToMap(new_x, new_y, x1, y1)) { return footprint_cells; } getLineCells(x0, x1, y0, y1, footprint_cells); } //we need to close the loop, so we also have to raytrace from the last pt to first pt new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th); new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th); if (!costmap.worldToMap(new_x, new_y, x0, y0)) { return footprint_cells; } new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th); new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th); if(!costmap.worldToMap(new_x, new_y, x1, y1)) { return footprint_cells; } getLineCells(x0, x1, y0, y1, footprint_cells); if(fill) { getFillCells(footprint_cells); } return footprint_cells; }
bool TableApproaches::tableApprCallback(hrl_table_detect::GetTableApproaches::Request& req, hrl_table_detect::GetTableApproaches::Response& resp) { double pose_step = 0.01, start_dist = 0.25, max_dist = 1.2, min_cost = 250; double close_thresh = 0.10; // TODO should this be transformed? std::vector<geometry_msgs::Point> table_poly = req.table.points; geometry_msgs::PointStamped approach_pt = req.approach_pt; /* double xsize = 1.0, ysize = 1.0, xoff = 2.5, yoff = 0.0; geometry_msgs::Point pt; pt.x = xoff-xsize/2; pt.y = yoff-ysize/2; table_poly.push_back(pt); pt.x = xoff+xsize/2; pt.y = yoff-ysize/2; table_poly.push_back(pt); pt.x = xoff+xsize/2; pt.y = yoff+ysize/2; table_poly.push_back(pt); pt.x = xoff-xsize/2; pt.y = yoff+ysize/2; table_poly.push_back(pt); geometry_msgs::PointStamped approach_pt; approach_pt.header.frame_id = "/base_link"; approach_pt.header.stamp = ros::Time::now(); approach_pt.point.x = 2.2; approach_pt.point.y = 0.3; */ costmap_ros->getCostmapCopy(costmap); world_model = new base_local_planner::CostmapModel(costmap); geometry_msgs::PoseArray valid_locs; valid_locs.header.frame_id = "/base_link"; valid_locs.header.stamp = ros::Time::now(); geometry_msgs::PoseArray invalid_locs; invalid_locs.header.frame_id = "/base_link"; invalid_locs.header.stamp = ros::Time::now(); geometry_msgs::PoseArray close_locs; close_locs.header.frame_id = "/base_link"; close_locs.header.stamp = ros::Time::now(); for(int i=0;i<4000;i++) { geometry_msgs::PoseStamped cpose, odom_pose; cpose.header.frame_id = "/base_link"; cpose.header.stamp = ros::Time(0); cpose.pose.position.x = (rand()%8000) / 1000.0 -4 ; cpose.pose.position.y = (rand()%8000) / 1000.0 - 4; double rot = (rand()%10000) / 10000.0 * 2 * 3.14; cpose.pose.orientation = tf::createQuaternionMsgFromYaw(rot); cout << cpose.pose.orientation.z << " " << cpose.pose.orientation.w << " " << rot << endl; tf_listener.transformPose("/odom_combined", cpose, odom_pose); //uint32_t x, y; //if(!costmap.worldToMap(odom_pose.pose.position.x, odom_pose.pose.position.y, x, y)) //continue; Eigen::Vector3f pos(odom_pose.pose.position.x, odom_pose.pose.position.y, tf::getYaw(odom_pose.pose.orientation)); //cout << x << ", " << y << ":" << curpt.point.x << "," << curpt.point.y << "$"; //cout << double(costmap.getCost(x,y)) << endl; double foot_cost = footprintCost(pos, 1); if(foot_cost == 0) valid_locs.poses.push_back(cpose.pose); else if(foot_cost != -1) close_locs.poses.push_back(cpose.pose); else invalid_locs.poses.push_back(cpose.pose); cout << foot_cost << endl; } cout << costmap_ros->getRobotFootprint().size() << endl; valid_pub.publish(valid_locs); invalid_pub.publish(invalid_locs); close_pub.publish(close_locs); geometry_msgs::PoseArray dense_table_poses; dense_table_poses.header.frame_id = "/base_link"; dense_table_poses.header.stamp = ros::Time::now(); uint32_t i2; for(uint32_t i=0;i<table_poly.size();i++) { i2 = i+1; if(i2 == table_poly.size()) i2 = 0; double diffx = table_poly[i2].x-table_poly[i].x; double diffy = table_poly[i2].y-table_poly[i].y; double len = std::sqrt(diffx*diffx + diffy*diffy); double ang = std::atan2(diffy, diffx) - 3.14/2; double incx = std::cos(ang)*0.01, incy = std::sin(ang)*0.01; for(double t=0;t<len;t+=pose_step) { double polyx = table_poly[i].x + t*diffx; double polyy = table_poly[i].y + t*diffy; geometry_msgs::PoseStamped test_pose, odom_test_pose; bool found_pose = false; for(int k=start_dist/0.01;k<max_dist/0.01;k++) { test_pose.header.frame_id = "/base_link"; test_pose.header.stamp = ros::Time(0); test_pose.pose.position.x = polyx + incx*k; test_pose.pose.position.y = polyy + incy*k; test_pose.pose.orientation = tf::createQuaternionMsgFromYaw(ang+3.14); tf_listener.transformPose("/odom_combined", test_pose, odom_test_pose); Eigen::Vector3f pos(odom_test_pose.pose.position.x, odom_test_pose.pose.position.y, tf::getYaw(odom_test_pose.pose.orientation)); double foot_cost = footprintCost(pos, 1.0); // found a valid pose if(foot_cost >= 0 && foot_cost <= min_cost) { found_pose = true; break; } uint32_t mapx, mapy; // break if we go outside the grid if(!costmap.worldToMap(odom_test_pose.pose.position.x, odom_test_pose.pose.position.y, mapx, mapy)) break; double occ_map = double(costmap.getCost(mapx, mapy)); // break if we come across and obstacle if(occ_map == costmap_2d::LETHAL_OBSTACLE || occ_map == costmap_2d::NO_INFORMATION) break; } if(found_pose) dense_table_poses.poses.push_back(test_pose.pose); } } ROS_INFO("POLY: %d, denseposes: %d", table_poly.size(), dense_table_poses.poses.size()); // downsample and sort dense pose possibilties by distance to // approach point and thresholded distance to each other geometry_msgs::PoseArray downsampled_table_poses; boost::function<bool(geometry_msgs::Pose&, geometry_msgs::Pose&)> dist_comp = boost::bind(&pose_dist_comp, _1, _2, approach_pt.point); while(ros::ok() && !dense_table_poses.poses.empty()) { // add the closest valid pose to the approach location on the table geometry_msgs::Pose new_pose = *std::min_element( dense_table_poses.poses.begin(), dense_table_poses.poses.end(), dist_comp); downsampled_table_poses.poses.push_back(new_pose); // remove all poses in the dense sampling which are close to // the newest added pose boost::function<bool(geometry_msgs::Pose)> rem_thresh = boost::bind(&pose_dist_thresh, _1, new_pose.position, close_thresh); dense_table_poses.poses.erase(std::remove_if( dense_table_poses.poses.begin(), dense_table_poses.poses.end(), rem_thresh), dense_table_poses.poses.end()); ROS_INFO("denseposes: %d", dense_table_poses.poses.size()); } downsampled_table_poses.header.frame_id = "/base_link"; downsampled_table_poses.header.stamp = ros::Time::now(); approach_pub.publish(downsampled_table_poses); resp.approach_poses = downsampled_table_poses; ROS_INFO("POLY: %d, poses: %d", table_poly.size(), downsampled_table_poses.poses.size()); delete world_model; 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(); }
void FootprintLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if(!enabled_) return; std::vector<geometry_msgs::Point> footprint_points = costmap_2d::toPointVector(footprint_.polygon); master_grid.setConvexPolygonCost(footprint_points, costmap_2d::FREE_SPACE); }
// method for applyting changes in this layer to global costmap void HANPLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { boost::recursive_mutex::scoped_lock lock(configuration_mutex_); if(!enabled_ || (lastTransformedHumans.size() == 0)) { return; } // update map according to each human position for(auto human : lastTransformedHumans) { // get position of human in map unsigned int cell_x, cell_y; if(!master_grid.worldToMap(human.pose.position.x, human.pose.position.y, cell_x, cell_y)) { ROS_ERROR("hanp_layer: no world coordinates for human at x:%f y:%f", human.pose.position.x, human.pose.position.y); continue; } // general algorithm is to // create satic grids according to human posture, standing, sitting, walking // to calcuate cost use a sigmoid function // however 'sitting' posture is not implemented yet if(use_safety) { if(safety_grid == NULL) { ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as safety_grid is empty"); return; } // apply safety grid according to position of each human auto size_x = (int)(safety_max / resolution), size_y = size_x; // by convention x is number of columns, y is number of rows for(int y = 0; y <= 2 * size_y; y++) { for(int x = 0; x <= 2 * size_x; x++) { // add to current cost auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y) + (double)(safety_grid[x + ((2 * size_x + 1) * y)] * safety_weight); // detect overflow if(cost > (int)costmap_2d::LETHAL_OBSTACLE) { cost = costmap_2d::LETHAL_OBSTACLE; } master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost); } } } if(use_visibility) { // calculate visibility grid double yaw = tf::getYaw(human.pose.orientation); auto visibility_grid = createVisibilityGrid(visibility_max, resolution, costmap_2d::LETHAL_OBSTACLE, cos(yaw), sin(yaw)); if(visibility_grid == NULL) { ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as visibility_grid is empty"); return; } // apply the visibility grid auto size_x = (int)(visibility_max / resolution), size_y = size_x; for(int y = 0; y <= 2 * size_y; y++) { for(int x = 0; x <= 2 * size_x; x++) { // add to current cost auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y) + (double)(visibility_grid[x + ((2 * size_x + 1) * y)] * visibility_weight); // detect overflow if(cost > (int)costmap_2d::LETHAL_OBSTACLE) { cost = costmap_2d::LETHAL_OBSTACLE; } master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost); } } } } // ROS_DEBUG("hanp_layer: updated costs"); }
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 getMin(costmap_2d::Costmap2D gradient_, int &i, int &j) { int iOut = i; int jOut = j; if(gradient_.getCost( i-1, j-1) < gradient_.getCost( iOut, jOut)) { iOut = i - 1; jOut = j - 1; } if(gradient_.getCost( i-1, j) < gradient_.getCost( iOut, jOut)) { iOut = i - 1; jOut = j; } if(gradient_.getCost( i-1, j+1) < gradient_.getCost( iOut, jOut)) { iOut = i - 1; jOut = j + 1; } if(gradient_.getCost( i, j-1) < gradient_.getCost( iOut, jOut)) { iOut = i; jOut = j - 1; } if(gradient_.getCost( i, j+1) < gradient_.getCost( iOut, jOut)) { iOut = i; jOut = j + 1; } if(gradient_.getCost( i+1, j-1) < gradient_.getCost( iOut, jOut)) { iOut = i + 1; jOut = j - 1; } if(gradient_.getCost( i+1, j) < gradient_.getCost( iOut, jOut)) { iOut = i + 1; jOut = j; } if(gradient_.getCost( i+1, j+1) < gradient_.getCost( iOut, jOut)) { iOut = i + 1; jOut = j + 1; } i = iOut; j = jOut; }
double TableApproaches::footprintCost(const Eigen::Vector3f& pos, double scale){ double cos_th = cos(pos[2]); double sin_th = sin(pos[2]); std::vector<geometry_msgs::Point> scaled_oriented_footprint; for(unsigned int i = 0; i < footprint_model.size(); ++i){ geometry_msgs::Point new_pt; new_pt.x = pos[0] + (scale * footprint_model[i].x * cos_th - scale * footprint_model[i].y * sin_th); new_pt.y = pos[1] + (scale * footprint_model[i].x * sin_th + scale * footprint_model[i].y * cos_th); scaled_oriented_footprint.push_back(new_pt); } geometry_msgs::Point robot_position; robot_position.x = pos[0]; robot_position.y = pos[1]; //check if the footprint is legal double footprint_cost = world_model->footprintCost(robot_position, scaled_oriented_footprint, costmap.getInscribedRadius(), costmap.getCircumscribedRadius()); return footprint_cost; }