void FrontierDetector::processFrontiers(const cv::Mat& occupancyGrid){
map = occupancyGrid.clone();
if(map.rows>0 && map.cols>0) {
imgProc();
detectEdges();
findFrontiers();
}
}
bool ExploreFrontier::getFrontiers(std::vector<geometry_msgs::Pose>& frontiers)
{
findFrontiers();
if (frontiers_.size() == 0)
return false;
frontiers.clear();
for (auto& frontier : frontiers_) {
frontiers.push_back(frontier.pose);
}
return true;
}
bool ExploreFrontier::getFrontiers(Costmap2DROS& costmap, std::vector<geometry_msgs::Pose>& frontiers)
{
findFrontiers(costmap);
if (frontiers_.size() == 0)
return false;
frontiers.clear();
for (uint i=0; i < frontiers_.size(); i++) {
geometry_msgs::Pose frontier;
frontiers.push_back(frontiers_[i].pose);
}
return (frontiers.size() > 0);
}
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);
}
