bool ExploreFrontier::getExplorationGoals( tf::Stamped<tf::Pose> robot_pose, std::vector<geometry_msgs::Pose>& goals, double potential_scale, double orientation_scale, double gain_scale) { findFrontiers(); if (frontiers_.size() == 0) { ROS_DEBUG("no frontiers found"); return false; } geometry_msgs::Point start; start.x = robot_pose.getOrigin().x(); start.y = robot_pose.getOrigin().y(); start.z = robot_pose.getOrigin().z(); planner_->computePotential(start); //we'll make sure that we set goals for the frontier at least the circumscribed //radius away from unknown space // 2015: this may not be necessary // float step = -1.0 * costmap_resolution; // int c = ceil(costmap.getCircumscribedRadius() / costmap_resolution); // WeightedFrontier goal; std::vector<WeightedFrontier> weightedFrontiers; weightedFrontiers.reserve(frontiers_.size()); for (auto& frontier: frontiers_) { WeightedFrontier weightedFrontier; weightedFrontier.frontier = frontier; tf::Point p(frontier.pose.position.x, frontier.pose.position.y, frontier.pose.position.z); tf::Quaternion bt; tf::quaternionMsgToTF(frontier.pose.orientation, bt); tf::Vector3 v(cos(bt.getAngle()), sin(bt.getAngle()), 0.0); // for (int j=0; j <= c; j++) { // tf::Vector3 check_point = p + (v * (step * j)); // weightedFrontier.frontier.pose.position.x = check_point.x(); // weightedFrontier.frontier.pose.position.y = check_point.y(); // weightedFrontier.frontier.pose.position.z = check_point.z(); // weightedFrontier.cost = potential_scale * getFrontierCost(weightedFrontier.frontier) + orientation_scale * getOrientationChange(weightedFrontier.frontier, robot_pose) - gain_scale * getFrontierGain(weightedFrontier.frontier, costmapResolution_); // // weightedFrontier.cost = getFrontierCost(weightedFrontier.frontier) - getFrontierGain(weightedFrontier.frontier, costmapResolution_); // // ROS_DEBUG("cost: %f (%f * %f + %f * %f - %f * %f)", // // weightedFrontier.cost, // // potential_scale, // // getFrontierCost(weightedFrontier.frontier), // // orientation_scale, // // getOrientationChange(weightedFrontier.frontier, robot_pose), // // gain_scale, // // getFrontierGain(weightedFrontier.frontier, costmapResolution_) ); // weightedFrontiers.push_back(weightedFrontier); // } weightedFrontier.cost = potential_scale * getFrontierCost(weightedFrontier.frontier) + orientation_scale * getOrientationChange(weightedFrontier.frontier, robot_pose) - gain_scale * getFrontierGain(weightedFrontier.frontier); weightedFrontiers.push_back(std::move(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); }
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); }