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 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); } }
//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); }
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; }
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!"); }
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); }
//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; }
// 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"); }
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; }