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); } };