void NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) { tf::Stamped<tf::Pose> global_pose; cmap_->getRobotPose(global_pose); vector<PoseStamped> path; rm::PoseStamped start; start.header.stamp = global_pose.stamp_; start.header.frame_id = global_pose.frame_id_; start.pose.position.x = global_pose.getOrigin().x(); start.pose.position.y = global_pose.getOrigin().y(); start.pose.position.z = global_pose.getOrigin().z(); start.pose.orientation.x = global_pose.getRotation().x(); start.pose.orientation.y = global_pose.getRotation().y(); start.pose.orientation.z = global_pose.getRotation().z(); start.pose.orientation.w = global_pose.getRotation().w(); makePlan(start, *goal, path); }
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) { frontiers_.clear(); Costmap2D costmap; costmap_.getCostmapCopy(costmap); int idx; int w = costmap.getSizeInCellsX(); int h = costmap.getSizeInCellsY(); int size = (w * h); int pts = 0; map_.info.width = w; map_.info.height = h; map_.set_data_size(size); map_.info.resolution = costmap.getResolution(); map_.info.origin.position.x = costmap.getOriginX(); map_.info.origin.position.y = costmap.getOriginY(); // Find all frontiers (open cells next to unknown cells). const unsigned char* map = costmap.getCharMap(); for (idx = 0; idx < size; idx++) { //get the world point for the index unsigned int mx, my; costmap.indexToCells(idx, mx, my); geometry_msgs::Point p; costmap.mapToWorld(mx, my, p.x, p.y); //check if the point has valid potential and is next to unknown space //bool valid_point = planner_->validPointPotential(p); //bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p); //bool valid_point = (map[idx] < LETHAL_OBSTACLE); //bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE ); bool valid_point = (map[idx] == FREE_SPACE); if ((valid_point && map) && (((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) || ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) || ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) || ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)))) { map_.data[idx] = -128; //ROS_INFO("Candidate Point %d.", pts++ ); } else { map_.data[idx] = -127; } } // Clean up frontiers detected on separate rows of the map // I don't know what this means -- Travis. idx = map_.info.height - 1; for (unsigned int y=0; y < map_.info.width; y++) { map_.data[idx] = -127; idx += map_.info.height; } // Group adjoining map_ pixels int segment_id = 127; std::vector< std::vector<FrontierPoint> > segments; for (int i = 0; i < size; i++) { if (map_.data[i] == -128) { std::vector<int> neighbors, candidates; std::vector<FrontierPoint> segment; neighbors.push_back(i); int points_in_seg = 0; unsigned int mx, my; geometry_msgs::Point p_init, p; costmap.indexToCells(i, mx, my); costmap.mapToWorld(mx, my, p_init.x, p_init.y); // claim all neighbors while (neighbors.size() > 0) { int idx = neighbors.back(); neighbors.pop_back(); btVector3 tot(0,0,0); int c = 0; if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) { tot += btVector3(1,0,0); c++; } if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) { tot += btVector3(-1,0,0); c++; } if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) { tot += btVector3(0,1,0); c++; } if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) { tot += btVector3(0,-1,0); c++; } assert(c > 0); // Only adjust the map and add idx to segment when its near enough to start point. // This should prevent greedy approach where all cells belong to one frontier! costmap.indexToCells(idx, mx, my); costmap.mapToWorld(mx, my, p.x, p.y); float dist; dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 )); if ( dist <= 0.8 ){ map_.data[idx] = segment_id; // This used to be up before "assert" block above. points_in_seg += 1; //ROS_INFO( "Dist to start cell: %3.2f", dist ); segment.push_back(FrontierPoint(idx, tot / c)); // consider 8 neighborhood if (((idx-1)>0) && (map_.data[idx-1] == -128)) neighbors.push_back(idx-1); if (((idx+1)<size) && (map_.data[idx+1] == -128)) neighbors.push_back(idx+1); if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128)) neighbors.push_back(idx-map_.info.width); if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128)) neighbors.push_back(idx-map_.info.width+1); if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128)) neighbors.push_back(idx-map_.info.width-1); if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128)) neighbors.push_back(idx+map_.info.width); if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128)) neighbors.push_back(idx+map_.info.width+1); if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128)) neighbors.push_back(idx+map_.info.width-1); } } segments.push_back(segment); ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg ); segment_id--; if (segment_id < -127) break; } } int num_segments = 127 - segment_id; ROS_INFO("Segments: %d", num_segments ); if (num_segments <= 0) return; for (unsigned int i=0; i < segments.size(); i++) { Frontier frontier; std::vector<FrontierPoint>& segment = segments[i]; uint size = segment.size(); //we want to make sure that the frontier is big enough for the robot to fit through // This seems like a goofy heuristic rather than fact. What happens if we don't do this...? if (size * costmap.getResolution() < costmap.getInscribedRadius()){ ROS_INFO( "Discarding segment... too small?" ); //continue; } float x = 0, y = 0; btVector3 d(0,0,0); for (uint j=0; j<size; j++) { d += segment[j].d; int idx = segment[j].idx; x += (idx % map_.info.width); y += (idx / map_.info.width); // x = (idx % map_.info.width); // y = (idx / map_.info.width); } d = d / size; frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size); frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size); // frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y); frontier.pose.position.z = 0.0; frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x())); frontier.size = size; geometry_msgs::Point p; p.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position p.y = map_.info.origin.position.y + map_.info.resolution * (y); if (!planner_->validPointPotential(p)){ ROS_INFO( "Discarding segment... can't reach?" ); //continue; } frontiers_.push_back(frontier); } }
bool ExploreFrontier::getExplorationGoals(Costmap2DROS& costmap, geometry_msgs::Point start, navfn::NavfnROS* planner, std::vector<geometry_msgs::Pose>& goals, double potential_scale, double orientation_scale, double gain_scale) { planner->computePotential(start); planner_ = planner; costmapResolution_ = costmap.getResolution(); findFrontiers(costmap); if (frontiers_.size() == 0){ return false; } //we'll make sure that we set goals for the frontier at least the circumscribed //radius away from unknown space double goal_stepback = costmap.getCircumscribedRadius(); WeightedFrontier goal; std::vector<WeightedFrontier> weightedFrontiers; weightedFrontiers.reserve(frontiers_.size()); for (uint i=0; i < frontiers_.size(); i++) { Frontier& frontier = frontiers_[i]; WeightedFrontier weightedFrontier; weightedFrontier.frontier = frontier; tf::Quaternion bt; tf::quaternionMsgToTF(frontier.pose.orientation, bt); double distance = 0.0; //we'll walk back along the potential function generated by the planner from the frontier point at least the inscribed radius while(distance < goal_stepback){ geometry_msgs::Point check_point = frontier.pose.position; check_point.x -= costmapResolution_; check_point.y -= costmapResolution_; double best_potential = planner->getPointPotential(check_point); geometry_msgs::Point best_point = check_point; //look at the neighbors of the current point to find the one with the lowest potential for(unsigned int i = 0; i < 3; ++i){ for(unsigned int j = 0; j < 3; ++j){ check_point.x += costmapResolution_; double potential = planner->getPointPotential(check_point); //check if the potential is better, make sure that we don't check the frontier point itself if(potential < best_potential && !(i == 1 && j == 1)){ best_potential = potential; best_point = check_point; } } check_point.x = frontier.pose.position.x - costmapResolution_; check_point.y += costmapResolution_; } //update the frontier position and add to the distance traveled frontier.pose.position.x = best_point.x; frontier.pose.position.y = best_point.y; distance += costmapResolution_; } //create a line using the vector we stored in the frontier //Line l(Point(frontier.pose.position.x, frontier.pose.position.y, frontier.pose.position.z), bt.getAngle()); //step back the inscribed radius along that line to find a goal point //Point t = l.eval(goal_stepback); weightedFrontier.frontier.pose.position.x = frontier.pose.position.x; weightedFrontier.frontier.pose.position.y = frontier.pose.position.y; weightedFrontier.frontier.pose.position.z = frontier.pose.position.z; tf::Stamped<tf::Pose> robot_pose; costmap.getRobotPose(robot_pose); //compute the cost of the frontier weightedFrontier.cost = potential_scale * getFrontierCost(weightedFrontier.frontier) + orientation_scale * getOrientationChange(weightedFrontier.frontier, robot_pose) - gain_scale * getFrontierGain(weightedFrontier.frontier, costmapResolution_); weightedFrontiers.push_back(weightedFrontier); } goals.clear(); goals.reserve(weightedFrontiers.size()); std::sort(weightedFrontiers.begin(), weightedFrontiers.end()); for (uint i = 0; i < weightedFrontiers.size(); i++) { goals.push_back(weightedFrontiers[i].frontier.pose); } return (goals.size() > 0); }