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