int Planner::computePlan(int horizon) {
mdp->initVI();
for (int h=0; h<horizon; ++h) {
int s;
#ifdef DEBUG_VI
std::cout << "H"<<h<<":";
#endif
mdp->iterate(patience);
if (publishPlan(h))
return h+1;
} // for h
return horizon;
} // computePlan(h)
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN(
"The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
timespec time1, time2;
/* take current time here */
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
std::cout<<"time to generate best global path time = " << (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 << " microseconds" <<"\n";
//data<<"["<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 ;
// add orientations if needed
orientation_filter_->processPath(start, plan);
float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.end(); ++it) {
std::cout<<"路径位姿点：("<<(*it).pose.position.x<<","<<(*it).pose.position.y<<")"<<"\n";
path_length += hypot( (*it).pose.position.x - last_pose.pose.position.x,
(*it).pose.position.y - last_pose.pose.position.y );
last_pose = *it;
}
std::cout <<"------The global path length: "<< path_length<< " meters------"<<"\n";
//data <<"["<< path_length<< ",";
//data<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
std::cout <<"["<< path_length<< ",";
std::cout<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
//función llamada por move_base para obtener el plan.
bool MyastarPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
//***********************************************************
// Inicio de gestion de ROS, mejor no tocar nada en esta parte
//***********************************************************
if(!initialized_){
ROS_ERROR("The astar planner has not been initialized, please call initialize() to use the planner");
return false;
}
ROS_INFO("MyastarPlanner: Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
plan.clear();
//obtenemos el costmap global que está publicado por move_base.
costmap_ = costmap_ros_->getCostmap();
//Obligamos a que el marco de coordenadas del goal enviado y del costmap sea el mismo.
//esto es importante para evitar errores de transformaciones de coordenadas.
if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
tf::Stamped<tf::Pose> goal_tf;
tf::Stamped<tf::Pose> start_tf;
poseStampedMsgToTF(goal,goal_tf);
poseStampedMsgToTF(start,start_tf);
//obtenemos la orientación start y goal en start_yaw y goal_yaw.
double useless_pitch, useless_roll, goal_yaw, start_yaw;
start_tf.getBasis().getEulerYPR(start_yaw, useless_pitch, useless_roll);
goal_tf.getBasis().getEulerYPR(goal_yaw, useless_pitch, useless_roll);
/**************************************************************************/
/*************** HASTA AQUÍ GESTIÓN DE ROS *********************************/
/****************************************************************************/
//pasamos el goal y start a un nodo (estructura coupleOfCells)
coupleOfCells cpstart, cpgoal;
double goal_x = goal.pose.position.x;
double goal_y = goal.pose.position.y;
unsigned int mgoal_x, mgoal_y;
costmap_->worldToMap(goal_x,goal_y,mgoal_x, mgoal_y);
cpgoal.index = MyastarPlanner::costmap_->getIndex(mgoal_x, mgoal_y);
cpgoal.parent=0;
cpgoal.gCost=0;
cpgoal.hCost=0;
cpgoal.fCost=0;
double start_x = start.pose.position.x;
double start_y = start.pose.position.y;
unsigned int mstart_x, mstart_y;
costmap_->worldToMap(start_x,start_y, mstart_x, mstart_y);
cpstart.index = MyastarPlanner::costmap_->getIndex(mstart_x, mstart_y);
cpstart.parent =cpstart.index;
cpstart.gCost = 0;
cpstart.hCost = MyastarPlanner::calculateHCost(cpstart.index,cpgoal.index);
double maxdistance=sqrt(costmap_->getSizeInMetersY()*costmap_->getSizeInMetersY()+costmap_->getSizeInMetersX()*costmap_->getSizeInMetersX());
//insertamos el nodo inicial en abiertos
MyastarPlanner::openList.insert(cpstart);
ROS_INFO("Inserto en Abiertos: %d", cpstart.index );
ROS_INFO("Index del goal: %d", cpgoal.index );
unsigned int explorados = 0;
unsigned int currentIndex = cpstart.index;
while (!MyastarPlanner::openList.empty()) //while the open list is not empty continuie the search
{
//escoger el nodo (coupleOfCells) de abiertos que tiene el valor más pequeño de f.
coupleOfCells COfCells=*(openList.begin());
currentIndex=COfCells.index;
//vamos a insertar ese nodo en cerrados
//obtenemos un iterador a ese nodo en la lista de abiertos
set<coupleOfCells>::iterator it=getPositionInList(openList,currentIndex);
//copiamos el contenido de ese nodo a una variable nodo auxiliar
cpstart.index=currentIndex;
cpstart.parent=(*it).parent;
cpstart.gCost=(*it).gCost;
cpstart.hCost=(*it).hCost;
cpstart.fCost=(*it).fCost;
//y esa variable la insertamos en cerrados
MyastarPlanner::closedList.insert(cpstart);
//ROS_INFO("Inserto en CERRADOS: %d", (*it).index );
ROS_INFO("G: %f, H: %f, F: %f", (*it).gCost, (*it).hCost, (*it).fCost);
ROS_INFO("Index: %d Parent: %d", (*it).index, (*it).parent);
// Si el nodo recién insertado es el goal, ¡plan encontrado!
if(currentIndex==cpgoal.index || explorados == 2000)
{
//el plan lo construimos partiendo del goal, del parent del goal y saltando en cerrados "de parent en parent"
//y vamos insertando al final los waypoints (los nodos de cerrados)
ROS_INFO("PLAN ENCONTRADO!!!");
//convertimos goal a poseStamped nueva
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = goal.header.frame_id;//debe tener el mismo frame que el goal pasado por parámetro
pose.pose.position.x = goal_x;
pose.pose.position.y = goal_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
//lo añadimos al plan%
plan.push_back(pose);
ROS_INFO("Inserta en Plan: %f, %f", pose.pose.position.x, pose.pose.position.y);
coupleOfCells currentCouple = cpstart;
unsigned int currentParent = cpstart.parent;
while (currentCouple.index != currentParent) //e.d. mientras no lleguemos al nodo start
{
//encontramos la posición de currentParent en cerrados
set<coupleOfCells>::iterator it=getPositionInList(closedList,currentParent);
//hacemos esa posición que sea el currentCouple
currentCouple.index=currentParent;
currentCouple.parent=(*it).parent;
currentCouple.gCost=(*it).gCost;
currentCouple.hCost=(*it).hCost;
currentCouple.fCost=(*it).fCost;
//creamos una PoseStamped con la información de currentCouple.index
//primero hay que convertir el currentCouple.index a world coordinates
unsigned int mpose_x, mpose_y;
double wpose_x, wpose_y;
costmap_->indexToCells(currentCouple.index, mpose_x, mpose_y);
costmap_->mapToWorld(mpose_x, mpose_y, wpose_x, wpose_y);
//después creamos la pose
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = goal.header.frame_id;//debe tener el mismo frame que el de la entrada
pose.pose.position.x = wpose_x;
pose.pose.position.y = wpose_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
//insertamos la pose en el plan
plan.push_back(pose);
ROS_INFO("Inserta en Plan: %f, %f", pose.pose.position.x, pose.pose.position.y);
//hacemos que currentParent sea el parent de currentCouple
currentParent = currentCouple.parent;
}
ROS_INFO("Sale del bucle de generación del plan.");
std::reverse(plan.begin(),plan.end());
//lo publica en el topic "planTotal"
publishPlan(plan);
return true;
}
//Si no hemos encontrado plan aún eliminamos el nodo insertado de ABIERTOS.
openList.erase(openList.begin());
//Buscamos en el costmap las celdas adyacentes a la actual
vector <unsigned int> neighborCells=findFreeNeighborCell(currentIndex);
//Ignoramos las celdas que ya existen en CERRADOS
for(int i = 0; i < neighborCells.size(); i++){
if( isContains(closedList, neighborCells[i]) ){
neighborCells.erase(neighborCells.begin()+i);
i--;
}
}
//Determinamos las celdas que ya están en ABIERTOS y las que no están en ABIERTOS
//Para los nodos que ya están en abiertos, comprobar en cerrados su coste y actualizarlo si fuera necesario
//Añadimos a ABIERTOS las celdas que todavía no están en ABIERTO, marcando el nodo actual como su padre
//ver la función addNeighborCellsToOpenList(openList, neighborsNotInOpenList, currentIndex, coste_del_nodo_actual, indice_del_nodo_goal);
addNeighborCellsToOpenList(openList, neighborCells, currentIndex, cpstart.gCost, cpgoal.index, maxdistance);
explorados++;
}
if(openList.empty()) // if the openList is empty: then failure to find a path
{
ROS_INFO("Failure to find a path !");
return false;
// exit(1);
}
};
bool EpicNavCorePlugin::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (!initialized) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: EpicNavCorePlugin has not been initialized yet.");
return false;
}
// Set the goal location on the potential map.
unsigned int x_coord = 0;
unsigned int y_coord = 0;
if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, x_coord, y_coord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert goal to cost map location.");
x_coord = 0;
y_coord = 0;
}
setGoal(x_coord, y_coord);
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Solving harmonic function...");
int result = harmonic_complete_gpu(&harmonic, NUM_THREADS_GPU);
if (result != EPIC_SUCCESS) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not execute GPU version of 'epic' library.");
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Trying CPU fallback...");
result = harmonic_complete_cpu(&harmonic);
}
if (result == EPIC_SUCCESS) {
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Successfully solved harmonic function!");
} else {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to solve harmonic function.");
return false;
}
//pub_potential.publish(harmonic.u);
plan.clear();
plan.push_back(start);
/*
if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, xCoord, yCoord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to cost map location.");
xCoord = 0;
yCoord = 0;
}
*/
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(start.pose.position.x, start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to floating point cost map location.");
}
float step_size = 0.05f;
float cd_precision = 0.5f;
unsigned int max_length = harmonic.m[0] * harmonic.m[1] / step_size;
unsigned int k = 0;
float *raw_plan = nullptr;
result = harmonic_compute_path_2d_cpu(&harmonic, x, y, step_size, cd_precision, max_length, k, raw_plan);
if (result != EPIC_SUCCESS) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to compute the path.");
if (raw_plan != nullptr) {
delete [] raw_plan;
}
return false;
}
geometry_msgs::PoseStamped new_goal = goal;
for (unsigned int i = 1; i < k; i++) {
float x = raw_plan[2 * i + 0];
float y = raw_plan[2 * i + 1];
float plan_theta = std::atan2(y - raw_plan[2 * (i - 1) + 1], x - raw_plan[2 * (i - 1) + 0]);
float plan_x = 0.0f;
float plan_y = 0.0f;
mapToWorld(x, y, plan_x, plan_y);
new_goal.pose.position.x = plan_x;
new_goal.pose.position.y = plan_y;
new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(plan_theta);
plan.push_back(new_goal);
}
delete [] raw_plan;
raw_plan = nullptr;
plan.push_back(goal);
publishPlan(plan);
return true;
}
