int main(int argc, char** argv)
{
ros::init(argc, argv, "pose_3d");
ros::NodeHandle node;
ros::Rate rate(10.0);
tf::TransformListener listener;
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::StampedTransform stransform;
tf::Quaternion q;
ros::Publisher path_pub = node.advertise<nav_msgs::Path>("/youbotPath", 15);
ros::Publisher grid_pub = node.advertise<nav_msgs::OccupancyGrid>("/nav_map", 15);
ros::Publisher mark_pub = node.advertise<visualization_msgs::MarkerArray>("/NBVs", 15);
//sleep(5);
ros::Subscriber sub = node.subscribe("/octomap_binary", 15, map_cb);
ros::Subscriber flag_sub = node.subscribe("/nbv_iter", 15, flag_cb);
ros::Subscriber grid_sub = node.subscribe("/projected_map", 15, grid_cb);
point3d sensorOffset(0.1, 0.3, 0);
point3d firstPos;
/*//Create transform from sensor to robot base
transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base", "xtion"));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "baseGoal", "sensorGoal"));
//-----------------------*/
const clock_t start = std::clock();
point3d curPose(-1.1, -0.05, 0.0);
vector<point3d> NBVList;
vector<point3d> PoseList;
visualization_msgs::MarkerArray views;
transform.setOrigin( tf::Vector3(curPose.x(), curPose.y(), 0) );
q.setRPY(0, 0, DEG2RAD(-90));
transform.setRotation(q);
ros::Time gTime = ros::Time::now();
br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
//Create transform from sensor to robot base
//transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
//q.setRPY(0, 0, 0);
//transform.setRotation(q);
//br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "xtion", "base"));
//br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "sensorGoal", "baseGoal"));
//-----------------------
sleep(1);
//int x;
//cin >> x;
//moveBase(origin, yaw, &listener);
cout << range << endl;
cout << sRes << endl;
//Setup Stuff
//OcTree tree(res);
cout << "Loading Sensor Model from File" << endl;
ifstream readEm("SensorModel.bin", ios::in | ios::binary);
while (!readEm.eof())
{
int buffer;
readEm.read((char*)&buffer, sizeof(int));
if (buffer < 0) break;
optiMap.push_back(buffer);
vector<int> tempRay;
while (!readEm.eof())
{
readEm.read((char*)&buffer, sizeof(int));
if (buffer < 0) break;
tempRay.push_back(buffer);
}
optiRays.push_back(tempRay);
}
readEm.close();
cout << "Finished Loading Sensor Model" << endl;
double tempRoll, tempPitch, tempYaw;
try{
gTime = ros::Time::now();
listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(15.0));
listener.lookupTransform("vicon", "xtion", gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d origin(stransform.getOrigin().x(), stransform.getOrigin().y(), stransform.getOrigin().z());
tf::Matrix3x3 rot(stransform.getRotation());
rot.getRPY(tempRoll, tempPitch, tempYaw);
NBVList.push_back(origin);
cout << "Starting Pose: " << tempYaw << " : " << tempPitch << endl;
visualization_msgs::Marker mark;
mark.header.frame_id = "vicon";
mark.header.stamp = ros::Time::now();
mark.ns = "basic_shapes";
mark.id = 0;
mark.type = visualization_msgs::Marker::ARROW;
mark.action = visualization_msgs::Marker::ADD;
mark.pose.position.x = origin.x();
mark.pose.position.y = origin.y();
mark.pose.position.z = origin.z();
q.setRPY(0, tempPitch, tempYaw);
mark.pose.orientation.x = q.getX();
mark.pose.orientation.y = q.getY();
mark.pose.orientation.z = q.getZ();
mark.pose.orientation.w = q.getW();
mark.scale.x = 0.2;
mark.scale.y = 0.02;
mark.scale.z = 0.02;
mark.color.r = 1.0f;
mark.color.g = 0.0f;
mark.color.a = 1.0;
mark.color.b = 0.0f;
views.markers.push_back(mark);
mark_pub.publish(views);
int NBVcount = 0;
while (ros::ok())
{
/*cout << "NBV " << NBVcount++ << endl;
mapReceived = false;
startAlg = false;
while ((!mapReceived || !startAlg) && ros::ok())
{
mark_pub.publish(views);
ros::spinOnce();
rate.sleep();
}
//AbstractOcTree
OcTree* treeTemp = binaryMsgToMap(mapMsg);
//OcTree treeTemp(0.01);
treeTemp->writeBinary("firstMap.bt");
cout << "Loaded the Map" << endl;
sleep(2);
try{
gTime = ros::Time::now();
listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(15.0));
listener.lookupTransform("vicon", "xtion", gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d camPosition (stransform.getOrigin().x(), stransform.getOrigin().y(), 0);
origin = NBVList.back();
cout << "Map Loaded " << treeTemp->getResolution() << endl;*/
point3d egoPose(0, 0, 0);
point3d endPoint(-1.4, -1.5, 0);
//pObj Tuning Coefficients
double alpha = 0.5;
double beta = 2.0;
size_t pointCount = 0;
//point3d origin = curPose;
point3d camPosition(0,0,0);
OcTree tree(res);
tree.setClampingThresMax(0.999);
tree.setClampingThresMin(0.001);
tree.setProbMiss(0.1);
tree.setProbHit(0.995);
tree.readBinary("firstMap.bt");
tree.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
bool flag = false;
for (vector<point3d>::iterator nbvIt = NBVList.begin(); nbvIt != NBVList.end(); nbvIt++)
{
if (iter.getCoordinate().distanceXY(*nbvIt) < 0.6)
{
flag = true;
}
}
if (tree.isNodeOccupied(*iter)) // && (iter.getZ() < 0.03 || iter.getX() < -2 || iter.getX() > 1 || iter.getY() < -1 || iter.getY() > 1) || flag)
{
tree.setNodeValue(iter.getKey(), -lEmpty);
tree.updateNode(iter.getKey(), false);
if (tree.isNodeOccupied(*iter))
{
cout << "Errrnk" << endl;
}
}
}
cout << origin.x() << ", " << origin.y() << endl;
origin = tree.keyToCoord(tree.coordToKey(origin));
cout << "Origin: " << origin.x() << " : " << origin.y() << " : " << origin.z() << endl;
if (NBVcount == 1)
{
firstPos = origin;
}
//Use Second Tree to store pObj entities for NBV prediction
OcTree tree2(tree);
string fileName = "algMap" + to_string(NBVcount) + ".bt";
tree.writeBinary(fileName);
//-------------------
//Cost Function Work - 2D Map Creation, Dijkstras Path
//-------------------
double robotRad = 0.52;
double minX, minY, minZ, maxX, maxY, maxZ;
tree.getMetricMin(minX, minY, minZ);
tree.getMetricMax(maxX, maxY, maxZ);
point3d minPoint(minX, minY, minZ);
point3d maxPoint(maxX, maxY, maxZ);
cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
minPoint = tree.keyToCoord(tree.coordToKey(minPoint));
minX = minPoint.x();
minY = minPoint.y();
minZ = minPoint.z();
maxPoint = tree.keyToCoord(tree.coordToKey(maxPoint));
maxX = maxPoint.x();
maxY = maxPoint.y();
maxZ = maxPoint.z();
cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
tree.expand();
int rangeX = round((maxX - minX) * rFactor);
int rangeY = round((maxY - minY) * rFactor);
vector<GridNode> gridMap(rangeX * rangeY, GridNode());
vector<int> freeMap(rangeX * rangeY, 0);
for (int x = 0; x < rangeX; x++)
{
for (int y = 0; y < rangeY; y++)
{
//cout << "XY: " << x << ", " << y << endl;
gridMap[rangeX * y + x].coords = point3d(double(x) * res + minX, double(y) * res + minY, 0);
for (int i = -1; i <= 1; i += 1)
{
for (int j = -1; j <= 1; j += 1)
{
if ((i != 0 || j != 0) && x + i >= 0 && x + i < rangeX && y + j >= 0 && y + j < rangeY)
{
gridMap[rangeX * y + x].neighbors.push_back(&gridMap[rangeX * (y + j) + x + i]);
gridMap[rangeX * y + x].edges.push_back(sqrt(pow(i, 2) + pow(j, 2)) * res);
//cout << gridMap[rangeX*y + x].neighbors.size() << ", " << gridMap[rangeX*y + x].edges.size() << endl;
}
}
}
}
}
cout << "First" << endl;
tree.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
/*if (!tree.isNodeOccupied(*iter) && iter.getZ() > 0)
{
int index = int((iter.getCoordinate().y() - minY) / res) * rangeX + (iter.getCoordinate().x() - minX) / res;
gridMap[index].object = true;
gridMap[index].occupied = true;
for (double offX = -robotRad; offX <= robotRad; offX += res)
{
for (double offY = -robotRad; offY <= robotRad; offY += res)
{
if (sqrt(pow(offX, 2) + pow(offY, 2)) <= robotRad)
{
int xInd = (gridMap[index].coords.x() + offX - minX) * rFactor;
int yInd = (gridMap[index].coords.y() + offY - minY) * rFactor;
if (xInd >= 0 && xInd < rangeX && yInd >= 0 && yInd < rangeY)
{
int offIdx = yInd * rangeX + xInd;
gridMap[offIdx].occupied = true;
}
}
}
}
}*/
if (!tree.isNodeOccupied(*iter) && iter.getZ() > 0 && iter.getZ() <= 0.5)
{
int index = round((iter.getCoordinate().y() - minY) / res) * rangeX + round((iter.getCoordinate().x() - minX) / res);
freeMap[index]++;
}
}
for (int index = 0; index < rangeX * rangeY; index++)
{
if (freeMap[index] < 20 && gridMap[index].coords.distanceXY(firstPos) > 0.8 && gridMap[index].coords.distanceXY(camPosition) > 0.2)
{
gridMap[index].object = true;
gridMap[index].occupied = true;
for (double offX = -robotRad; offX <= robotRad; offX += res)
{
for (double offY = -robotRad; offY <= robotRad; offY += res)
{
if (sqrt(pow(offX, 2) + pow(offY, 2)) <= robotRad)
{
int xInd = round((gridMap[index].coords.x() + offX - minX) * rFactor);
int yInd = round((gridMap[index].coords.y() + offY - minY) * rFactor);
if (xInd >= 0 && xInd < rangeX && yInd >= 0 && yInd < rangeY)
{
int offIdx = yInd * rangeX + xInd;
if (gridMap[offIdx].coords.distanceXY(firstPos) > 0.4 && gridMap[offIdx].coords.distanceXY(camPosition) > 0.3)
{
gridMap[offIdx].occupied = true;
}
}
}
}
}
}
}
cout << "Second" << endl;
cout << rangeX << ", " << rangeY << " : " << int((origin.x() - minX) * rFactor) << ", " << int((origin.y() - minY) * rFactor) << endl;
int origIdx = rangeX * int((origin.y() - minY) / res) + int((origin.x() - minX) / res);
//round(((origin.y() - minY) * rFactor) * rangeX + (origin.x() - minX) * rFactor);
cout << origIdx << " " << gridMap.size() << endl;
GridNode* originNode = &gridMap[origIdx];
originNode->cost = 0;
cout << "Got Here" << endl;
MinHeap heapy;
heapy.Push(originNode);
while (heapy.GetLength() > 0)
{
GridNode* minNode = heapy.Pop();
vector<float>::iterator edgeIt = minNode->edges.begin();
for (vector<GridNode*>::iterator iter = minNode->neighbors.begin(); iter != minNode->neighbors.end(); iter++, edgeIt++)
{
GridNode* nodePt = *iter;
if ((*iter)->occupied == 0 && (*iter)->cost > (minNode->cost + *edgeIt))
{
(*iter)->parent = minNode;
if ((*iter)->state == 0)
{
(*iter)->cost = minNode->cost + *edgeIt;
heapy.Push((*iter));
(*iter)->state = 1;
}
else if ((*iter)->state == 1)
{
heapy.Update((*iter)->heapIdx, minNode->cost + *edgeIt);
}
}
}
}
cout << "Dijkstra Path Complete" << endl;
nav_msgs::OccupancyGrid grid;
grid.header.stamp = ros::Time::now();
grid.header.frame_id = "vicon";
nav_msgs::MapMetaData metas;
metas.resolution = res;
metas.origin.position.x = minX;
metas.origin.position.y = minY;
metas.origin.position.z = 0.07;
metas.height = rangeY;
metas.width = rangeX;
grid.info = metas;
cout << gridMap.size() << endl;
for (vector<GridNode>::iterator iter = gridMap.begin(); iter != gridMap.end(); iter++)
{
if (iter->occupied)
{
grid.data.push_back(100);
}
else
{
grid.data.push_back(0);
}
}
grid_pub.publish(grid);
cout << "Map Published" << endl;
pointCount = 0;
int cubeCount = 0;
int emptyCount = 0;
/*tree.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
if (tree.isNodeOccupied(*iter))
{
pointCount++;
}
else
{
emptyCount++;
}
}*/
cout << cubeCount << " cubes" << endl;
cout << "Empties: " << emptyCount << endl;
//OcTreeCustom infoTree(res);
//infoTree.readBinary("kinect.bt");
//Copy all occupancy values from known nodes into pObj variables for those nodes
//infoTree.expand();
/*for (OcTreeCustom::iterator iter = infoTree.begin(); iter != infoTree.end(); iter++)
* { *
* iter->setpObj(float(iter->getOccupancy()));
}*/
int cellCount = 0;
int unkCount = 0;
//pointCount = 0;
cout << "Beginning pObj Processing" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
OcTreeKey occKey = iter.getKey();
OcTreeKey unKey;
OcTreeKey freeKey;
OcTreeNode *cellNode;
OcTreeNode *unNode;
if (tree.isNodeOccupied(*iter))
{
cellCount++;
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
for (int k = -1; k <= 1; k++)
{
if (i != 0 || j != 0 || k != 0)
{
unKey = occKey;
unKey[0] += i;
unKey[1] += j;
unKey[2] += k;
unNode = tree.search(unKey);
if (unNode == NULL)
{
bool critical = false;
for (int u = -1; u <= 1; u++)
{
for (int v = -1; v <= 1; v++)
{
for (int w = -1; w <= 1; w++)
{
if (abs(u) + abs(v) + abs(w) == 1)
{
freeKey = unKey;
freeKey[0] += u;
freeKey[1] += v;
freeKey[2] += w;
cellNode = tree.search(freeKey);
if (cellNode != NULL && !tree.isNodeOccupied(cellNode) && tree.keyToCoord(freeKey).z() > 0.15)
{//unKey is a critical unknown cell, proceed to update pObj of unknown cells in vicinity
critical = true;
}
}
}
}
}
if (critical)
{
//cout << "Occ: " << tree.keyToCoord(occKey).x() << ", " << tree.keyToCoord(occKey).y() << ", " << tree.keyToCoord(occKey).z() << endl;
//cout << "Unk: " << tree.keyToCoord(unKey).x() << ", " << tree.keyToCoord(unKey).y() << ", " << tree.keyToCoord(unKey).z() << endl;
tree2.updateNode(tree2.coordToKey(tree.keyToCoord(unKey)), (float)log(alpha / (1 - alpha)));
//New method using custom tree
//infoTree.updateNode(tree.keyToCoord(unKey), float(0));
//infoTree.search(tree.keyToCoord(unKey))->setpObj((float)log(alpha / (1 - alpha)));
unkCount++;
for (double boxX = -5 * res; boxX <= 5 * res; boxX += res)
{
for (double boxY = -5 * res; boxY <= 5 * res; boxY += res)
{
for (double boxZ = -5 * res; boxZ <= 5 * res; boxZ += res)
{
point3d objPoint = tree.keyToCoord(unKey) + point3d(boxX, boxY, boxZ);
if (objPoint.z() > 0.05)
{
double pObj = alpha*exp(-sqrt(pow(boxX, 2) + pow(boxY, 2) + pow(boxZ, 2)) * beta);
cellNode = tree.search(objPoint);
if (cellNode == NULL)
{
if (tree2.search(objPoint) == NULL)
{
tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
pointCount++;
}
else
{
if ((tree2.search(objPoint))->getLogOdds() < (float)log(pObj / (1 - pObj)))
{
tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
}
//tree2.updateNode(objPoint, max((tree2.search(objPoint))->getLogOdds(), (float)log2(pObj / (1 - pObj))));
}
}
/*else
* {
* tree2.setNodeValue(objPoint, cellNode->getLogOdds());
*
* infoTree.search(objPoint)->setpObj(cellNode->getOccupancy());
}*/
}
}
}
}
}
}
}
}
}
}
}
}
/*infoTree.expand();
*
* f *or (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
* {
* iter->updateOccupancyChildren();
* if (iter->getLogOdds() != 0 && abs(iter->getOccupancy() - iter->getpObj()) > 0.01)
* {
* cout << iter->getLogOdds() << ", " << iter->getOccupancy() << ", " << iter->getpObj() << endl;
* addCube(iter.getCoordinate(), res, 0, 0, 1, 0.5);
}
if (iter->getLogOdds() == 0 && iter->getpObj() > 0.001)
{
addCube(iter.getCoordinate(), sRes, 0, 1, 0, 0.5);
}
}*/
/*tree.expand();
tree2.expand();
for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
{
if (tree2.search(iter.getCoordinate())->getOccupancy() != iter->getOccupancy())
{
cout << tree2.search(iter.getCoordinate())->getOccupancy() << ", " << iter->getOccupancy() << endl;
}
}*/
cout << "Nothing Different" << endl;
tree2.writeBinary("objMap.bt");
pointCount = 0;
tree2.expand();
KeyRay cellList;
point3d egoCOG(2.95, 1.95, 0);
Quaternion egoTheta(point3d(0, 0, -M_PI / 2));
endPoint = point3d(3.0, 0, 0);
OcTreeKey myCell;
OcTreeNode *cellNode;
double cellLike;
Pose6D trialPose(egoCOG, egoTheta);
endPoint = trialPose.transform(endPoint);
//Pre-Computing Table Stuff
point3d rayOrigin(0.005, 0.005, 0.005);
point3d rayInit(nRayLength * res, 0, 0);
point3d rayEnd;
/*vector<int> castMap;
* v *ector<int> logAdd;
*
* cellCount = 0;
* int traversed = 0;
*
* Pointcloud castCloud;
*
* for (double yaw = 0; yaw < 360; yaw++)
* {
* cout << yaw << endl;
* for (double pitch = -90; pitch <= 90; pitch++)
* {
* Quaternion rayTheta(point3d(0, DEG2RAD(pitch), DEG2RAD(yaw)));
* Pose6D rayPose(rayOrigin, rayTheta);
* rayEnd = rayPose.transform(rayInit);
*
* cellList.reset();
* tree.computeRayKeys(rayOrigin, rayEnd, cellList);
* //cout << "cellList Size: " << cellList.size() << endl;
* for (KeyRay::iterator iter = ++cellList.begin(); iter != cellList.end(); iter++)
* {
* traversed++;
* point3d rayCoord = tree.keyToCoord(*iter) - rayOrigin;
*
* if (rayCoord.norm() * sRes / res > 0.6)
* {
* int index = round(rayCoord.x() / res) + nRayLength + (2 * nRayLength + 1) * round(rayCoord.y() / res + nRayLength) + pow(2 * nRayLength + 1, 2) * round(rayCoord.z() / res + nRayLength);
* vector<int>::iterator findIt = find(castMap.begin(), castMap.end(), index);
* if (findIt == castMap.end())
* {
* castMap.push_back(index);
* castCloud.push_back(rayCoord + rayOrigin);
* logAdd.push_back(lFilled);
* cellCount++;
}
else
{
int foundIndex = distance(castMap.begin(), findIt);
logAdd[foundIndex] += lFilled;
}
}
}
}
}
cout << "Cells in CastMap: " << cellCount << "out of " << traversed << "cells traversed" << endl;
cout << "Min Index: " << *min_element(castMap.begin(), castMap.end()) << endl;
cout << "Max Index: " << *max_element(castMap.begin(), castMap.end()) << endl;
cout << "Max logAdd: " << *max_element(logAdd.begin(), logAdd.end()) << endl;
cout << nRayLength << endl;
cout << "Cloud Size: " << castCloud.size() << endl;
cout << "Inserting Cloud..." << endl;
tree.insertPointCloud(castCloud, rayOrigin);
tree.writeBinary("castMap.bt");
//Write pre-computed tables to files for quick retrieval
ofstream outFile("mapCast.bin", ios::out | ios::binary);
for (vector<int>::iterator iter = castMap.begin(); iter != castMap.end(); iter++)
{
outFile.write((char*)&(*iter), sizeof(int));
}
outFile.close();
outFile = ofstream("logAdd.bin", ios::out | ios::binary);
for (vector<int>::iterator iter = logAdd.begin(); iter != logAdd.end(); iter++)
{
outFile.write((char*)&(*iter), sizeof(int));
}
outFile.close();
cout << "Done Writing" << endl;
while (true)
{
}*/
//--------------------
//Create 3D Arrays of object information for quick retrieval in infoGain calcs
//--------------------
//First pass to find limits of info bounding box
double temp_x, temp_y, temp_z;
tree2.getMetricMax(temp_x, temp_y, temp_z);
//infoTree.getMetricMin(temp_x, temp_y, temp_z);
ibxMin = point3d(temp_x, temp_y, temp_z);
tree2.getMetricMin(temp_x, temp_y, temp_z);
//infoTree.getMetricMax(temp_x, temp_y, temp_z);
ibxMax = point3d(temp_x, temp_y, temp_z);
for (OcTree::iterator iter = tree2.begin_leafs(searchDepth); iter != tree2.end_leafs(); iter++)
{
if (iter->getOccupancy() > 0.001)
{
//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
ibxMin.x() = min(ibxMin.x(), iter.getCoordinate().x());
ibxMin.y() = min(ibxMin.y(), iter.getCoordinate().y());
ibxMin.z() = min(ibxMin.z(), iter.getCoordinate().z());
ibxMax.x() = max(ibxMax.x(), iter.getCoordinate().x());
ibxMax.y() = max(ibxMax.y(), iter.getCoordinate().y());
ibxMax.z() = max(ibxMax.z(), iter.getCoordinate().z());
}
}
/*for (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
* { *
* iter->updateOccupancyChildren();
* if (iter->getpObj() > 0.001)
* {
* if (iter.getCoordinate().x() < ibxMin.x()) ibxMin.x() = iter.getCoordinate().x();
* if (iter.getCoordinate().y() < ibxMin.y()) ibxMin.y() = iter.getCoordinate().y();
* if (iter.getCoordinate().z() < ibxMin.z()) ibxMin.z() = iter.getCoordinate().z();
*
* if (iter.getCoordinate().x() > ibxMax.x()) ibxMax.x() = iter.getCoordinate().x();
* if (iter.getCoordinate().y() > ibxMax.y()) ibxMax.y() = iter.getCoordinate().y();
* if (iter.getCoordinate().z() > ibxMax.z()) ibxMax.z() = iter.getCoordinate().z();
}
}*/
cout << "Min Corner: " << ibxMin.x() << ", " << ibxMin.y() << ", " << ibxMin.z() << endl;
cout << "Max Corner: " << ibxMax.x() << ", " << ibxMax.y() << ", " << ibxMax.z() << endl;
cout << "S: " << sFactor << endl;
//Determine required size of infoChunk array and initialize accordingly
boxX = round((ibxMax.x() - ibxMin.x()) / sRes + 1);
boxY = round((ibxMax.y() - ibxMin.y()) / sRes + 1);
boxZ = round((ibxMax.z() - ibxMin.z()) / sRes + 1);
cout << boxX << ", " << boxY << ", " << boxZ << endl;
infoChunk = vector<vector<float> >(boxX * boxY * boxZ, { 0 , 0 });
//Second pass to populate 3D array with info values
tree2.expand();
for (OcTree::leaf_bbx_iterator iter = tree2.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree2.end_leafs_bbx(); iter++)
{
if (iter.getCoordinate().z() > 0.05 && iter->getOccupancy() > 0.001)
{
int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
int index = xInd + yInd * boxX + zInd * boxX * boxY;
//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
infoChunk[index][1] = iter->getOccupancy();
infoChunk[index][0] = iter->getOccupancy();
}
}
tree.expand();
for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree.end_leafs_bbx(); iter++)
{
int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
int index = xInd + yInd * boxX + zInd * boxX * boxY;
if (iter.getCoordinate().z() > 0.05 && tree.isNodeOccupied(*iter))
{
double p = exp(iter->getLogOdds()) / (1 + exp(iter->getLogOdds()));
infoChunk[index][0] = (-p * log2(p) - (1 - p) * log2(1 - p)) * infoChunk[index][1];
//infoChunk[index][0] = iter->getLogOdds();
}
else
{
infoChunk[index][0] = 0;
}
}
/*for (OcTreeCustom::leaf_bbx_iterator iter = infoTree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != infoTree.end_leafs_bbx(); iter++)
* { *
* iter->updateOccupancyChildren();
* if (iter->getpObj() > 0.001)
* {
* int xInd = round((iter.getCoordinate().x() - ibxMin.x()) / sRes);
* int yInd = round((iter.getCoordinate().y() - ibxMin.y()) / sRes);
* int zInd = round((iter.getCoordinate().z() - ibxMin.z()) / sRes);
*
* cout << iter->getLogOdds() << endl;
* infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][0] = iter->getLogOdds();
* infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][1] = iter->getpObj();
}
}*/
cout << "infoChunk populated, uploading sensor model" << endl;
cout << "Begin Test" << endl;
//Search Algorithm
vector<InfoNode> gains;
float maxHeight = 0.62;
float minHeight = 0.28;
pointCount = 0;
double maxIG = 0;
double maxCost = 0;
double maxPitch = 0;
double maxYaw = 0;
point3d bestPos(0, 0, 0);
int viewCount = 0;
int searchCount = 0;
point3d searchBoxMin = ibxMin - point3d(1.5, 1.5, 1.5);
point3d searchBoxMax = ibxMax + point3d(1.5, 1.5, 1.5);
searchBoxMin.z() = max(searchBoxMin.z(), minHeight);
searchBoxMax.z() = min(searchBoxMax.z(), maxHeight);
cout << "Search Space: " << searchBoxMin.x() << ", " << searchBoxMin.y() << ", " << searchBoxMin.z() << endl;
cout << "To: " << searchBoxMax.x() << ", " << searchBoxMax.y() << ", " << searchBoxMax.z() << endl;
//-------------
//Heuristic Search Method
//-------------
/*const clock_t searchStart = clock();
*
* i *nt neighbDist = 2;
* double neighborStepSize = 0.02;
* vector<InfoNode> neighbors(6 * neighbDist, InfoNode());
*
* tree.expand();
* double xMax, yMax, zMax, xMin, yMin, zMin;
* tree.getMetricMax(xMax, yMax, zMax);
* tree.getMetricMin(xMin, yMin, zMin);
* for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 10); iter != tree.end_leafs_bbx(); iter++)
* {
* if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;
*
* searchCount++;
}
cout << searchCount << " positions to calculate" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(11); iter != tree.end_leafs(); iter++)
{
if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;
if (viewCount % 100 == 0)
{
cout << viewCount << endl;
}
point3d nextPos = iter.getCoordinate();
float pitch, yaw;
double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);
if (infoGain > 0)
{
gains.push_back(InfoNode(infoGain, 0, nextPos, yaw, pitch));
if (infoGain > maxIG)
{
maxIG = infoGain;
bestPos = nextPos;
maxYaw = yaw;
maxPitch = pitch;
}
//addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
}
viewCount++;
}
cout << "First Pass: "******" views" << endl;
sort(gains.begin(), gains.end());
vector<InfoNode>::iterator testIt = gains.end()--;
vector<InfoNode> searchTails;
vector<InfoNode>::iterator iter = gains.end()--;
for (int i = 0; i < 5; i++)
{
cout << i << endl;
InfoNode curNode = *iter;
while (true)
{
cout << ".";
point3d curPoint = curNode.coords;
double curGain = curNode.infoGain;
vector<InfoNode>::iterator neighbIt = neighbors.begin();
for (int offX = -neighbDist; offX <= neighbDist; offX++)
{
for (int offY = -neighbDist; offY <= neighbDist; offY++)
{
for (int offZ = -neighbDist; offZ <= neighbDist; offZ++)
{
if ((offX != 0) + (offY != 0) + (offZ != 0) != 1) continue;
point3d neighbor = curPoint + point3d(offX, offY, offZ) * neighborStepSize;
neighbIt->coords = neighbor;
if (neighbor.z() < 0 || neighbor.x() < xMin || neighbor.x() > xMax || neighbor.y() < yMin || neighbor.y() > yMax || neighbor.z() < zMin || neighbor.z() > zMax || tree.search(neighbor) == NULL || tree.isNodeOccupied(tree.search(neighbor)))
{
neighbIt->infoGain = 0;
}
else
{
float pitch, yaw;
neighbIt->infoGain = getInfoGain(&tree, &tree2, neighbor, &yaw, &pitch);
viewCount++;
neighbIt->pitch = pitch;
neighbIt->yaw = yaw;
}
neighbIt++;
}
}
}
InfoNode maxNeighb = *max_element(neighbors.begin(), neighbors.end());
if (maxNeighb.infoGain <= curGain)
{
break;
}
else
{
curNode = maxNeighb;
}
}
searchTails.push_back(curNode);
iter--;
cout << endl;
}
InfoNode hNBV = *max_element(searchTails.begin(), searchTails.end());
const clock_t searchEnd = clock();
cout << "Heuristic NBV: " << hNBV.coords.x() << ", " << hNBV.coords.y() << ", " << hNBV.coords.z() << endl;
cout << "With Info Gain = " << hNBV.infoGain << endl;
cout << "In " << double(searchEnd - searchStart) / CLOCKS_PER_SEC << " seconds, for " << viewCount << "candidate viewpoints" << endl;*/
//Original Grid Search
time_t time1 = time(0);
viewCount = 0;
tree.expand();
for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 14); iter != tree.end_leafs_bbx(); iter++)
{
if (!tree.isNodeOccupied(*iter))
{
searchCount++;
}
}
vector<double> infoMap(rangeX * rangeY, 0);
cout << searchCount << " positions to calculate" << endl;
for (OcTree::leaf_iterator iter = tree.begin_leafs(14); iter != tree.end_leafs(); iter++)
{
if (tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate())) || iter.getCoordinate().z() < minHeight || iter.getCoordinate().z() > maxHeight) continue;
if (viewCount % 100 == 0)
{
cout << viewCount << endl;
}
point3d nextPos = iter.getCoordinate();
float pitch, yaw;
double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);
if (infoGain > 0)
{
double moveCost = gridMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)].cost;
if (infoGain > infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)])
{
infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)] = infoGain;
}
gains.push_back(InfoNode(infoGain, moveCost, nextPos, yaw, pitch));
if (infoGain > maxIG)
{
maxIG = infoGain;
bestPos = nextPos;
maxYaw = yaw;
maxPitch = pitch;
}
if (moveCost > maxCost && moveCost < 600)
{
maxCost = moveCost;
}
/*infoCloud->points[pointCount].x = nextPos.x();
* infoCloud->points[pointCount].y = nextPos.y();
* infoCloud->points[pointCount].z = nextPos.z();
* infoCloud->points[pointCount].intensity = infoGain;
* pointCount++;*/
//addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
}
viewCount++;
}
cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
cout << "The NBV is at (" << bestPos.x() << ", " << bestPos.y() << ", " << bestPos.z() << ") with an expected info gain of " << maxIG << endl;
cout << maxYaw << ", " << maxPitch << endl;
cout << "Max Cost: " << maxCost << endl;
double maxQual = 0;
double correctIG = 0;
point3d NBV = origin;
for (vector<InfoNode>::iterator iter = gains.begin(); iter != gains.end(); iter++)
{
//iter->infoGain /= maxIG;
//iter->cost /= maxCost;
if (iter->cost < 600) iter->quality = iter->infoGain - 0 * iter->cost;
else iter->quality = 0;
if (iter->quality > maxQual)
{
maxQual = iter->quality;
correctIG = iter->infoGain;
NBV = iter->coords;
maxYaw = iter->yaw;
maxPitch = iter->pitch;
}
}
cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
cout << "The corrected NBV is at (" << NBV.x() << ", " << NBV.y() << ", " << NBV.z() << ") with a quality of " << maxQual << "and IG of " << correctIG << endl;
cout << "Yaw: " << maxYaw << ", " << "Pitch: " << maxPitch << endl;
point3d oldBest = bestPos;
if (maxIG < 10)
{
cout << "Algorithm Complete" << endl;
break;
}
NBVList.push_back(NBV);
visualization_msgs::Marker mark;
mark.header.frame_id = "vicon";
mark.header.stamp = ros::Time::now();
mark.ns = "basic_shapes";
mark.id = NBVcount;
mark.type = visualization_msgs::Marker::ARROW;
mark.action = visualization_msgs::Marker::ADD;
mark.pose.position.x = NBV.x();
mark.pose.position.y = NBV.y();
mark.pose.position.z = NBV.z();
q.setRPY(0, DEG2RAD(-maxPitch), DEG2RAD(maxYaw));
mark.pose.orientation.x = q.getX();
mark.pose.orientation.y = q.getY();
mark.pose.orientation.z = q.getZ();
mark.pose.orientation.w = q.getW();
mark.scale.x = 0.2;
mark.scale.y = 0.02;
mark.scale.z = 0.02;
mark.color.r = 0.0f;
mark.color.g = 1.0f;
mark.color.a = 1.0;
mark.color.b = 0.0f;
views.markers.push_back(mark);
mark_pub.publish(views);
/*transform.setOrigin( tf::Vector3(NBV.x(), NBV.y(), 0) );
q.setRPY(0, 0, DEG2RAD(maxYaw));
transform.setRotation(q);
gTime = ros::Time::now();
br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
sleep(1);
try{
listener.lookupTransform("vicon", "baseGoal",
gTime, stransform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
point3d baseGoal(stransform.getOrigin().x(), stransform.getOrigin().y(), 0);
m = tf::Matrix3x3(stransform.getRotation());
double bRoll, bPitch, bYaw;
m.getRPY(bRoll, bPitch, bYaw);*/
point3d baseGoal = NBV;
GridNode* endNode = &gridMap[rangeX * int((baseGoal.y() - minY) / res) + int((baseGoal.x() - minX) / res)];
vector<point3d> waypoints;
while (endNode != NULL)
{
//cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
//cout << "Cost: " << endNode->cost << endl;
endNode->cost = 0;
waypoints.push_back(endNode->coords);
//cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
endNode = endNode->parent;
}
reverse(waypoints.begin(), waypoints.end());
nav_msgs::Path path;
path.header.stamp = ros::Time::now();
path.header.frame_id = "vicon";
int state = 0;
int oldState = -1;
for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = path.header.stamp;
pose.header.frame_id = path.header.frame_id;
pose.pose.position.x = iter->x();
pose.pose.position.y = iter->y();
pose.pose.position.z = NBV.z();
pose.pose.orientation.y = maxPitch;
pose.pose.orientation.z = maxYaw;
if (path.poses.size() > 0)
{
if (iter->x() == path.poses.back().pose.position.x)
{
state = 0;
}
else if (iter->y() == path.poses.back().pose.position.y)
{
state = 1;
}
else
{
state = 2;
}
}
//cout << "State, Old State, Size: " << state << " : " << oldState << " : " << path.poses.size() << endl;
if (path.poses.size() > 1 && state == oldState)
{
path.poses.pop_back();
//cout << pose.pose.position.x << ", " << pose.pose.position.y << endl;
}
path.poses.push_back(pose);
oldState = state;
}
/*for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = path.header.stamp;
pose.header.frame_id = path.header.frame_id;
pose.pose.position.x = iter->x();
pose.pose.position.y = iter->y();
pose.pose.position.z = NBV.z();
pose.pose.orientation.y = maxPitch;
pose.pose.orientation.z = maxYaw;
path.poses.push_back(pose);
//cout << path.poses.back().position.x << ", " << path.poses.back().position.y << endl;
}*/
cout << "Path Message Generated" << endl;
path_pub.publish<nav_msgs::Path>(path);
ros::spinOnce();
cout << "Path Message Published" << endl;
}
while (ros::ok())
{
mark_pub.publish(views);
rate.sleep();
ros::spinOnce();
}
return 0;
}
void nav_callback(const projeto::QuadStatus status)
{
struct timeval stop, start;
gettimeofday(&start, NULL);
//do stuff
double timestamp = status.header.stamp.toSec();
theta = Vector3d(status.theta.x, status.theta.y, status.theta.z);
Vector3d x_new(status.position.x, status.position.y, status.position.z);
Vector3d vel(status.vel.x, status.vel.y, status.vel.z);
//theta(0) = degree_to_rad(msg_in.rotX);
//theta(1) = degree_to_rad(msg_in.rotY);
//theta(2) = degree_to_rad(msg_in.rotZ);
//ROS_INFO("I heard ax: [%f] ay: [%f] az: [%f]", vx_, vy_, vz_);
//Vector3d velV (vx_, vy_, vz_);
//Quaternion<double> rotQ = rotation(theta);
//Matrix3d R = rotQ.matrix();
//Vector3d vel = R * velV;
//pthread_mutex_lock(&mutex_1);
//float dt = timestamp - previous_tm; //geting dt in secs
if (x_new(0) < -10 || x_new(0) > 10 || x_new(1) < -10 || x_new(1) > 10) {
f_vector_print("theta", theta);
f_vector_print("x", x);
f_vector_print("x_new", x_new);
return;
}
x = x_new;
previous_tm = timestamp;
// pthread_mutex_unlock(&mutex_1);
Vector3d future_position;
vector<Vector3d> trajectory = predict_trajectory2(vel, x, timestamp, timestamp + TIME_AHEAD, TRAJECTORY_DT, future_position);
float short_dist = MAX_DIST;
nav_msgs::GridCells gcells;
vector<geometry_msgs::Point> obstaclerepo;
if (!production_mode) {
sensor_msgs::PointCloud pc;
pc.header.frame_id = "/nav";
pc.header.stamp = ros::Time();
pc.channels.resize(1);
pc.channels[0].name="trajectory";
pc.channels[0].values.resize(trajectory.size());
pc.points.resize(trajectory.size());
int i = 0;
for (vector<Vector3d>::iterator it=trajectory.begin(); it!=trajectory.end(); ++it) {
Vector3d pos = *it;
pc.channels[0].values[i] = 0;
pc.points[i].x = pos(0);
pc.points[i].y = pos(1);
pc.points[i].z = pos(2);
i++;
}
pub_pc2.publish(pc);
gcells.header.frame_id = "/nav";
gcells.header.stamp = ros::Time();
gcells.cell_width = OCTREE_RESOLUTION;
gcells.cell_height = OCTREE_RESOLUTION;
}
if (control_mode) {
// cout << "TIMESTAMP " << timestamp << endl;
// cout << "CONTADOR " << contador << endl;
// cout << "LIMIT " << CONTROL_LIMIT << endl;
if (timestamp < contador + CONTROL_LIMIT) {
double u_x = pid_x.getCommand(pos_obj(0) - x(0), timestamp);
double u_y = pid_y.getCommand(pos_obj(1) - x(1), timestamp);
double u_z = pid_z.getCommand(pos_obj(2) - x(2), timestamp);
double u_yaw = pid_yaw.getCommand(yaw_obj - theta(2), timestamp);
double cx = within(cos(theta(2)) * u_x + sin(theta(2)) * u_y, -1, 1);
double cy = within(-sin(theta(2)) * u_x + cos(theta(2)) * u_y, -1, 1);
double cz = within(u_z, -1, 1);
double cyaw = within(u_yaw, -1, 1);
// f_vector_print("objetivo", pos_obj);
// f_vector_print("posicao", x);
cout << "SENDING AUTOMATIC CONTROLS" << endl;
Command cmd(cx, cy, cz, cyaw);
send_velocity_command(cmd);
} else {
control_mode = 0;
cout << "CONTROL MODE OFF" << endl;
send_collision_mode_msg(false);
pid_x.reset();
pid_y.reset();
pid_z.reset();
pid_yaw.reset();
}
} else {
OcTreeKey bbxMinKey, bbxMaxKey;
//Vector3d lim1 = x + R * Vector3d(0, -DELTA_VOL/2, -1.0);
//Vector3d lim2 = x + R * Vector3d(DELTA_VOL, DELTA_VOL/2, 1.0);
point3d min_vol = point3d(x(0)-DELTA_VOL/2, x(1) -DELTA_VOL/2, x(2)-1.0);
point3d max_vol = point3d(x(0)+DELTA_VOL/2, x(1) +DELTA_VOL/2, x(2)+1.0);
tree.coordToKeyChecked(min_vol, bbxMinKey);
tree.coordToKeyChecked(max_vol, bbxMaxKey);
for(OcTree::leaf_bbx_iterator it = tree.begin_leafs_bbx(bbxMinKey, bbxMaxKey), end_bbx = tree.end_leafs_bbx(); it!= end_bbx; ++it){
//cout << "passei" << endl;
point3d coords = it.getCoordinate();
Vector3d wrapped_coords = Vector3d(coords(0), coords(1), coords(2));
//if (!in_perimeter(x, wrapped_coords, 0.5)) {
if (!production_mode) {
geometry_msgs::Point cell;
cell.x = coords(0);
cell.y = coords(1);
cell.z = coords(2);
obstaclerepo.push_back(cell);
}
if (it->getValue() > OCCUPIED_PROB) {
for (vector<Vector3d>::iterator it=trajectory.begin(); it!=trajectory.end(); ++it) {
Vector3d pos = *it;
if (there_will_be_collision(pos, wrapped_coords)) {
float dist = (wrapped_coords - x).norm();
cout << "DIST " << dist << endl;
if (dist < short_dist) {
short_dist = dist;
}
break;
}
}
}
//manipulate node, e.g.:
//cout << "Node center: " << it.getCoordinate() << endl;
//cout << "Node size: " << it.getSize() << endl;
//cout << "Node value: " << it->getValue() << endl;
if (short_dist < MAX_DIST) {
float ttc = short_dist/vel.norm();
if (ttc < TTC_LIMIT) {
pos_obj = x;
yaw_obj = atan2(sin(theta(2)),cos(theta(2)));
send_collision_mode_msg(true);
control_mode = 1;
contador = timestamp;
cout << "CONTROL MODE ON" << endl;
pid_x.reset();
pid_y.reset();
pid_z.reset();
pid_yaw.reset();
}
}
}
//}
}
if (!production_mode) {
int count_cells = 0;
gcells.cells.resize(obstaclerepo.size());
while (!obstaclerepo.empty()) {
gcells.cells[count_cells++] = obstaclerepo.back();
obstaclerepo.pop_back();
}
//pub_grid_cell.publish(gcells);
}
gettimeofday(&stop, NULL);
previous_vel = vel;
previous_theta = theta;
//previous_omega = omega;
gettimeofday(&stop, NULL);
//cout << "time took: "<< stop.tv_sec - start.tv_sec << "." << (stop.tv_usec - start.tv_usec)/1000000 << "s" << endl;
}