Qt::MatchFlags SearchOptions::getMatchFlags() const
{
// *** match flags ***
// MatchExactly = 0, Performs QVariant-based matching.
// MatchContains = 1,
// MatchStartsWith = 2,
// MatchEndsWith = 3,
// MatchRegExp = 4,
// MatchWildcard = 5,
// MatchFixedString = 8, Performs string-based matching.
// MatchCaseSensitive = 16,
// MatchWrap = 32,
// MatchRecursive = 64 Searches the entire hierarchy, but I do not have heirarchical data.
Qt::MatchFlags flags = Qt::MatchExactly;
if (isMatchEntireString()) flags |= Qt::MatchFixedString;
if (isContains()) flags |= Qt::MatchContains;
if (isStartsWith()) flags |= Qt::MatchStartsWith;
if (isEndsWith()) flags |= Qt::MatchEndsWith;
//if (isMatchAsString()) flags |= Qt::MatchFixedString;
if (isRegularExpression()) flags |= Qt::MatchRegExp;
if (isWildCard()) flags |= Qt::MatchWildcard;
if (isCaseSensitive()) flags |= Qt::MatchCaseSensitive;
if (isWrap()) flags |= Qt::MatchWrap;
return flags;
}
/**
* @brief notifyStatusChange メソッドを呼ぶことなく、アイテムを追加する
* @param item 追加するアイテム
* @return 追加されたかどうか。既に選択状態になっていた場合は false が返る
*/
inline bool silentAdd(const VSQ_NS::Event *event) {
if (!isContains(event)) {
eventItemList.insert(std::make_pair(event, *event));
return true;
} else {
return false;
}
}
QString SearchOptions::serializeSettings() const
{
QStringList list;
if (isMatchEntireString()) list << "MatchEntireString";
if (isContains()) list << "Contains";
if (isStartsWith()) list << "StartsWith";
if (isEndsWith()) list << "EndsWith";
if (isMatchAsString()) list << "AsString";
if (isRegularExpression()) list << "RegExp";
if (isWildCard()) list << "Wildcard";
list << (isCaseSensitive() ? "CaseSensitive" : "CaseInSensitive");
list << (isWrap() ? "Wrap" : "NoWrap");
list << (isBackwards() ? "Backward" : "Forward");
list << (isAllColumns() ? "AllColumns" : "OneColumn");
if (isReplace()) list << "Replace";
if (isReplaceAll()) list << "ReplaceAll";
return list.join(",");
}
//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);
}
};
