Node * Astar::startSearch(Pose start,double targetCov, int coord) { int ID = 1; int NodesExpanded = 0; if(this->tree.size() > 0) this->tree.clear(); if(!openList) { openList = new LList; } if(!closedList) { closedList = new LList; } // Be sure that open and closed lists are empty openList->free(); closedList->free(); if (!this->search_space) // Make sure that we have a map to search { // LOG(Logger::Warning,"Read the map and generate SearchSpace before Searching !!!") return NULL; } this->start.p.position.x = start.p.position.x; this->start.p.position.y = start.p.position.y; this->start.p.position.z = start.p.position.z; this->start.p.orientation.x = start.p.orientation.x; this->start.p.orientation.y = start.p.orientation.y; this->start.p.orientation.z = start.p.orientation.z; this->start.p.orientation.w = start.p.orientation.w; this->targetCov = targetCov; std::cout<<"\n --->>> Search Started <<<---"; findRoot(); // std::cout<<"\n"<<QString(" ---->>>Target is Set to be X=%1 Y=%2 Z=%3<<<---").arg(end.p.position.x).arg(end.p.position.y).arg(end.p.position.z).toStdString(); openList->add(root); // add the root to OpenList // while openList is not empty int count = 0; ros::Time search_begin = ros::Time::now(); while (openList->Start != NULL) { if((count++%1) == 0) { displayTree(); } current = openList->getHead(); // Get the node with the highest cost (first node) (it was the cheapest one before since we were taking the lower cost but now it is converted to a reward function) openList->next(); // Move to the next Node NodesExpanded++; // We reached the target pose, so build the path and return it. ros::Time reached_check_begin = ros::Time::now(); if (surfaceCoverageReached(current) && current!= root)//change goalReached to surfaceCoverageReached { // build the complete path to return // qDebug("Last Node destination: %f %f",current->pose.p.x(),current->pose.p.y()); current->next = NULL;//the last node in the path covFilteredCloud->points = current->cloud_filtered->points; //getting the accumelated cloud to check the coverage and display it in the test code std::cout<<"*************commulative distance : "<<current->distance<<"************ \n"; std::cout<<"*************commulative coverage : "<<current->coverage<<"************ \n"; std::cout<<"\n"<<QString(" --->>> Goal state reached with :%1 nodes created and :%2 nodes expanded <<<---").arg(ID).arg(NodesExpanded).toStdString(); // qDebug(" --->>> General Clean UP <<<---"); fflush(stdout); // int m=0; // while (p != NULL) // { // cout<<"\n --->>> Step["<<++m<<"] X="<<p->pose.p.x()<<" Y="<<p->pose.p.y(); // //cout<<"\n --->>> Angle is="<<RTOD(p->angle); // fflush(stdout); // p = p->parent; // } // Going up to the Root p = current; path = NULL; while (p != NULL) { // cout<<"\n Am Still HERE Step["<<++m<<"] X="<<p->pose.x<<" Y="<<p->pose.y; // fflush(stdout); // remove the parent node from the closed list (where it has to be) if(p->prev != NULL) (p->prev)->next = p->next; if(p->next != NULL) (p->next)->prev = p->prev; // check if we're removing the top of the list if(p == closedList->Start) closedList->next(); // set it up in the path p->next = path; path = p; p = p->parent; } // now delete all nodes on OPEN and Closed Lists openList->free(); closedList->free(); return path; } ros::Time reached_check_end = ros::Time::now(); double check_elapsed = reached_check_end.toSec() - reached_check_begin.toSec(); std::cout<<"surface coverage check duration: "<< check_elapsed <<"\n"; // Create List of Children for the current NODE if(!(childList = makeChildrenNodes(current))) // No more Children => Search Ended Unsuccessfully at this path Branch { std::cout<<"\n --->>> Search Ended On this Branch / We Reached a DEAD END <<<---"; } ros::Time make_children_end = ros::Time::now(); double make_elapsed = make_children_end.toSec() - reached_check_end.toSec(); std::cout<<"make children duration: "<< make_elapsed <<"\n"; // insert the children into the OPEN list according to their f values ros::Time childrentest_begin = ros::Time::now(); std::cout<<" <<<<<<<<<<<<<<<<<<<<<<<<<<<< new children SET >>>>>>>>>>>>>\n"; while (childList != NULL) { ros::Time childtest_begin = ros::Time::now(); curChild = childList; childList = childList->next; // set up the rest of the child node details curChild->parent = current; curChild->depth = current->depth + 1; curChild->id = ID++; curChild->next = NULL; curChild->prev = NULL; //************displaying the tested child point*********** // std::vector<geometry_msgs::Point> pts; // geometry_msgs::Point pt; // pt.x = curChild->pose.p.position.x; pt.y = curChild->pose.p.position.y; pt.z = curChild->pose.p.position.z; // pts.push_back(pt); // visualization_msgs::Marker ptsList = drawPoints(pts,2,1000000000); // testPointPub.publish(ptsList); //************voxelgrid*********** ros::Time all_begin = ros::Time::now(); pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ> temp_cloud, collective_cloud; ros::Time extract_begin = ros::Time::now(); temp_cloud = obj->extractVisibleSurface(curChild->senPose.p); ros::Time extract_end = ros::Time::now(); double extract_elapsed = extract_end.toSec() - extract_begin.toSec(); std::cout<<"Extract visible surface duration: "<< extract_elapsed <<"\n"; collective_cloud.points = curChild->parent->cloud_filtered->points; // std::cout<<"Parent cloud size: "<<collective_cloud.size()<<"\n"; // std::cout<<"child cloud size: "<<temp_cloud.size()<<"\n"; collective_cloud +=temp_cloud; // std::cout<<"collective cloud size: "<<collective_cloud.size()<<"\n"; tempCloud->points = collective_cloud.points; ros::Time filtering_begin = ros::Time::now(); pcl::VoxelGrid<pcl::PointXYZ> voxelgrid; voxelgrid.setInputCloud (tempCloud); voxelgrid.setLeafSize (0.5f, 0.5f, 0.5f); voxelgrid.filter (*curChild->cloud_filtered); ros::Time filtering_end = ros::Time::now(); double filtering_elapsed = filtering_end.toSec() - filtering_begin.toSec(); std::cout<<"filtering duration: "<< filtering_elapsed <<"\n"; // std::cout<<"\nchild collective cloud after filtering size: "<<curChild->cloud_filtered->size()<<"\n"; ros::Time calc_begin = ros::Time::now(); curChild->coverage = obj->calcCoveragePercent(curChild->cloud_filtered); ros::Time calc_end = ros::Time::now(); double calc_elapsed = calc_end.toSec() - calc_begin.toSec(); std::cout<<"calculate percentage duration: "<< calc_elapsed <<"\n"; ros::Time all_end = ros::Time::now(); double all_elapsed = all_end.toSec() - all_begin.toSec(); std::cout<<"all the part duration: "<< all_elapsed <<"\n\n"; curChild->distance = curChild->parent->distance + Dist(curChild->pose.p,curChild->parent->pose.p); // curChild->g_value = heuristic->gCost(curChild); curChild->h_value = heuristic->hCost(curChild); curChild->f_value = curChild->h_value;//curChild->g_value + curChild->h_value; Node * p; // check if the child is already in the open list // std::cout<<" OPEN LIST NODES\n"<<std::endl; // openList->print(); if (debug == true) { std::cout<<"curChildren f value= "<<curChild->f_value<<"\n"; std::cout<<"\n Finding the curchild : "<<curChild->pose.p.position.x<<" "<< curChild->pose.p.position.y<<" "<<curChild->pose.p.position.z<<" "<<curChild->pose.p.orientation.x<<" "<<curChild->pose.p.orientation.y<<" "<<curChild->pose.p.orientation.z<<" "<<curChild->pose.p.orientation.w<< std::endl; } if( (p = openList->find(curChild))) { if (debug == true) std::cout<<"check if the child is already in the open list"<<"\n"; if (p->f_value >=curChild->f_value)// it was <= (to take least cost) but now it is changed to be reward function && (p->direction == curChild->direction)) { if (debug == true) std::cout<<"Free the node the openlist check"<<"\n"; freeNode(curChild); curChild = NULL; } // the child is a shorter path to this point, delete p from the closed list else if (p->f_value < curChild->f_value )//&& (p->direction == curChild->direction))//************IMPORTANT****************** { if (debug == true) std::cout<<"removing the p from the openlist"<<"\n"; openList->remove(p); // std::cout<<"*****SELECTED child h value: "<<curChild->f_value<<"********\n"; // std::cout<<"parent :"<<"position x:"<<curChild->pose.p.position.x<<" y:"<<curChild->pose.p.position.y<<" z:"<<curChild->pose.p.position.z<<"\n"; // std::cout<<"parent :"<<"orientation x:"<<curChild->pose.p.orientation.x<<" y:"<<curChild->pose.p.orientation.y<<" z:"<<curChild->pose.p.orientation.z<<" w:"<<curChild->pose.p.orientation.w<<"\n"; //cout<<"\n --->>> Opened list -- Node is deleted, current child X="<<curChild->pose.x<<" Y="<<curChild->pose.y<<" has shorter path<<<---"; fflush(stdout); } } // test whether the child is in the closed list (already been there) if (curChild) { if (debug == true) { std::cout<<" check if the current child in the closed list\n"<<std::endl; std::cout<<" CLOSED LIST NODES\n"<<std::endl; closedList->print(); std::cout<<" Finding the curchild : "<<curChild->pose.p.position.x<<" "<< curChild->pose.p.position.y<<" "<<curChild->pose.p.position.z<<" "<<curChild->pose.p.orientation.x<<" "<<curChild->pose.p.orientation.y<<" "<<curChild->pose.p.orientation.z<<" "<<curChild->pose.p.orientation.w<< std::endl; } if((p = closedList->find(curChild))) { if (debug == true) std::cout<<"the child is already in the closed list"<<"\n"; if (p->f_value >=curChild->f_value)// && p->direction == curChild->direction)//************IMPORTANT****************** { if (debug == true) std::cout<<"Free the node the closed list check, parent is bigger than the child"<<"\n"; freeNode(curChild); curChild = NULL; } // the child is a shorter path to this point, delete p from the closed list else { if (debug == true) std::cout<<"the parent f value is less than the child"<<"\n"; /* This is the tricky part, it rarely happens, but in my case it happenes all the time :s * Anyways, we are here cause we found a better path to a node that we already visited, we will have to * Update the cost of that node and ALL ITS DESCENDENTS because their cost is parent dependent ;) * Another Solution is simply to comment everything and do nothing, doing this, the child will be added to the * Open List and it will be investigated further later on. */ //TODO : this SHOULD be fixed, very very DODGY // Node *ptr = closedList->Start; // while(ptr) // { // if(ptr->parent == p) // ptr->parent = NULL; // ptr = ptr->next; // } // closedList->Remove(p); //cout<<"\n --->>> Closed list -- Node is deleted, current child X="<<curChild->pose.x<<" Y="<<curChild->pose.y<<" has shorter path<<<---"; fflush(stdout); } } if (debug == true) std::cout<<"DID NOT find the child in the closed list\n"; } ros::Time current_end = ros::Time::now(); double current_elapsed = current_end.toSec() - search_begin.toSec(); if (debug == true) { std::cout<<"\n\n\n#####################################################################################\n"; std::cout<<"#################Duration of the SEARCH till now (s)= "<<current_elapsed<<"####################\n\n\n"; } // ADD the child to the OPEN List if (curChild) { if (debug == true) std::cout<<"adding the cur child to the openlist"<<"\n"; openList->add(curChild); } ros::Time childtest_end = ros::Time::now(); double childtest_elapsed = childtest_end.toSec() - childtest_begin.toSec(); if (debug == true) std::cout<<"****Child Test duration (s)= "<<childtest_elapsed<<"****\n"; } ros::Time childrentest_end = ros::Time::now(); double childrentest_elapsed = childrentest_end.toSec() - childrentest_begin.toSec(); // if (debug == true) std::cout<<"****Children Test duration (s) of node "<<current->id<<"= "<<childrentest_elapsed<<"****\n\n\n"; ros::Time search_end = ros::Time::now(); double current_elapsed1 = search_end.toSec() - search_begin.toSec(); if (debug == true) { std::cout<<"\n\n\n\n******************************************************************************************\n"; std::cout<<"*********************SEARCH DURATION (s)= "<<current_elapsed1<<"*****************\n\n\n\n"; } // put the current node onto the closed list, ==>> already visited List closedList->add(current); // Test to see if we have expanded too many nodes without a solution if (current->id > this->MAXNODES) { // LOG(Logger::Info,QString(" --->>> Expanded %d Nodes which is more than the maximum allowed MAXNODE=%1 , Search Terminated").arg(current->id,MAXNODES)) //Delete Nodes in Open and Closed Lists std::cout<<"the closed list and open list is freed"<<"\n"; closedList->free(); openList->free(); path = NULL; return path; // Expanded more than the maximium nodes state } } //... end of OPEN loop /* if we got here, then there is no path to the goal * delete all nodes on CLOSED since OPEN is now empty */ if (debug == true) std::cout<<"counter for displaying the tree: "<<count<<" \n"; closedList->free(); std::cout<<"\n --->>>No Path Found<<<---"; return NULL; }
ccQuadric* ccQuadric::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/) { //number of points unsigned count = cloud->size(); if (count < CC_LOCAL_MODEL_MIN_SIZE[HF]) { ccLog::Warning(QString("[ccQuadric::fitTo] Not enough points in input cloud to fit a quadric! (%1 at the very least are required)").arg(CC_LOCAL_MODEL_MIN_SIZE[HF])); return 0; } //project the points on a 2D plane CCVector3 G, X, Y, N; { CCLib::Neighbourhood Yk(cloud); //plane equation const PointCoordinateType* theLSQPlane = Yk.getLSQPlane(); if (!theLSQPlane) { ccLog::Warning("[ccQuadric::Fit] Not enough points to fit a quadric!"); return 0; } assert(Yk.getGravityCenter()); G = *Yk.getGravityCenter(); //local base N = CCVector3(theLSQPlane); assert(Yk.getLSQPlaneX() && Yk.getLSQPlaneY()); X = *Yk.getLSQPlaneX(); //main direction Y = *Yk.getLSQPlaneY(); //secondary direction } //project the points in a temporary cloud ccPointCloud tempCloud("temporary"); if (!tempCloud.reserve(count)) { ccLog::Warning("[ccQuadric::Fit] Not enough memory!"); return 0; } cloud->placeIteratorAtBegining(); for (unsigned k=0; k<count; ++k) { //projection into local 2D plane ref. CCVector3 P = *(cloud->getNextPoint()) - G; tempCloud.addPoint(CCVector3(P.dot(X),P.dot(Y),P.dot(N))); } CCLib::Neighbourhood Zk(&tempCloud); { //set exact values for gravity center and plane equation //(just to be sure and to avoid re-computing them) Zk.setGravityCenter(CCVector3(0,0,0)); PointCoordinateType perfectEq[4] = { 0, 0, 1, 0 }; Zk.setLSQPlane( perfectEq, CCVector3(1,0,0), CCVector3(0,1,0), CCVector3(0,0,1)); } uchar hfdims[3]; const PointCoordinateType* eq = Zk.getHeightFunction(hfdims); if (!eq) { ccLog::Warning("[ccQuadric::Fit] Failed to fit a quadric!"); return 0; } //we recenter the quadric object ccGLMatrix glMat(X,Y,N,G); ccBBox bb = tempCloud.getBB(); CCVector2 minXY(bb.minCorner().x,bb.minCorner().y); CCVector2 maxXY(bb.maxCorner().x,bb.maxCorner().y); ccQuadric* quadric = new ccQuadric(minXY, maxXY, eq, hfdims, &glMat); quadric->setMetaData(QString("Equation"),QVariant(quadric->getEquationString())); //compute rms if necessary if (rms) { const uchar& dX = hfdims[0]; const uchar& dY = hfdims[1]; const uchar& dZ = hfdims[2]; *rms = 0; for (unsigned k=0; k<count; ++k) { //projection into local 2D plane ref. const CCVector3* P = tempCloud.getPoint(k); PointCoordinateType z = eq[0] + eq[1]*P->u[dX] + eq[2]*P->u[dY] + eq[3]*P->u[dX]*P->u[dX] + eq[4]*P->u[dX]*P->u[dY] + eq[5]*P->u[dY]*P->u[dY]; *rms += (z-P->z)*(z-P->z); } if (count) { *rms = sqrt(*rms / count); quadric->setMetaData(QString("RMS"),QVariant(*rms)); } } return quadric; }
// find the nearest node to the start void Astar::findRoot() throw (SSPPException) { SearchSpaceNode * temp; if(!this->search_space) { throw(SSPPException((char*)"No SearchSpace Defined")); return; } double distance,shortest_distance = 100000; // allocate and setup the root node root = new Node; temp = this->search_space; while(temp!=NULL) { distance = Dist(temp->location,start.p); // Initialize the root node information and put it in the open list if (distance < shortest_distance) { shortest_distance = distance; root->pose.p.position.x = temp->location.position.x; root->pose.p.position.y = temp->location.position.y; root->pose.p.position.z = temp->location.position.z;//newly added root->pose.p.orientation.x = temp->location.orientation.x; root->pose.p.orientation.y = temp->location.orientation.y; root->pose.p.orientation.z = temp->location.orientation.z; root->pose.p.orientation.w = temp->location.orientation.w; root->senPose.p.position.x = temp->sensorLocation.position.x; root->senPose.p.position.y = temp->sensorLocation.position.y; root->senPose.p.position.z = temp->sensorLocation.position.z; root->senPose.p.orientation.x = temp->sensorLocation.orientation.x; root->senPose.p.orientation.y = temp->sensorLocation.orientation.y; root->senPose.p.orientation.z = temp->sensorLocation.orientation.z; root->senPose.p.orientation.w = temp->sensorLocation.orientation.w; root->id = temp->id; } temp = temp->next; } //************voxelgrid*********** pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ> temp_cloud; temp_cloud = obj->extractVisibleSurface(root->senPose.p);//extract visible surface at the sensor position tempCloud->points = temp_cloud.points; pcl::VoxelGrid<pcl::PointXYZ> voxelgrid; voxelgrid.setInputCloud (tempCloud); voxelgrid.setLeafSize (0.5f, 0.5f, 0.5f); voxelgrid.filter (*root->cloud_filtered); std::cout<<"root cloud before filtering size: "<<tempCloud->size()<<"\n"; std::cout<<"root cloud after filtering size: "<<root->cloud_filtered->size()<<"\n"; root->id = 0; root->parent = NULL; root->next = NULL; root->prev = NULL; root->g_value = 0; root->distance = 0; root->coverage =0; root->h_value = 0;//heuristic->gCost(root); root->f_value = root->g_value + root->h_value; root->depth = 0; // root->pose.phi = start.phi; // root->direction = FORWARD; //Translate(root->pose,start.phi); std::cout<<"\n"<<QString(" ---->>>Root is Set to be X=%1 Y=%2 Z=%3").arg(root->pose.p.position.x).arg(root->pose.p.position.y).arg(root->pose.p.position.z).toStdString(); }