int main(int argc, char** argv) { //############################################################## string btFilename = ""; unsigned char maxDepth = 16; // test timing: timeval start; timeval stop; const unsigned char tree_depth(16); const unsigned int tree_max_val(32768); double time_it, time_depr; if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){ printUsage(argv[0]); } btFilename = std::string(argv[1]); if (argc > 2){ maxDepth = (unsigned char)atoi(argv[2]); } maxDepth = std::min((unsigned char)16,maxDepth); if (maxDepth== 0) maxDepth = tree_depth; // iterate over empty tree: OcTree emptyTree(0.2); EXPECT_EQ(emptyTree.size(), 0); EXPECT_EQ(emptyTree.calcNumNodes(), 0); size_t iteratedNodes = 0; OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth); OcTree::tree_iterator t_end = emptyTree.end_tree(); EXPECT_TRUE (t_it == t_end); for( ; t_it != t_end; ++t_it){ iteratedNodes++; } EXPECT_EQ(iteratedNodes, 0); for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){ iteratedNodes++; } EXPECT_EQ(iteratedNodes, 0); cout << "\nReading OcTree file\n===========================\n"; OcTree* tree = new OcTree(btFilename); if (tree->size()<= 1){ std::cout << "Error reading file, exiting!\n"; return 1; } size_t count; std::list<OcTreeVolume> list_depr; std::list<OcTreeVolume> list_iterator; /** * get number of nodes: */ gettimeofday(&start, NULL); // start timer size_t num_leafs_recurs = tree->getNumLeafNodes(); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timer size_t num_leafs_it = 0; for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) { num_leafs_it++; } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: " <<time_it << " / " << time_depr << "\n========================\n\n"; /** * get all occupied leafs */ point3d tree_center; tree_center(0) = tree_center(1) = tree_center(2) = (float) (((double) tree_max_val) * tree->getResolution()); gettimeofday(&start, NULL); // start timer getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timer for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){ if(tree->isNodeOccupied(*it)) { //count ++; list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Occupied lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; /** * get all free leafs */ list_iterator.clear(); list_depr.clear(); gettimeofday(&start, NULL); // start timer for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) { if(!tree->isNodeOccupied(*it)) list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); gettimeofday(&start, NULL); // start timer getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); std::cout << "Free lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; /** * get all volumes */ list_iterator.clear(); list_depr.clear(); gettimeofday(&start, NULL); // start timer getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution()); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timers for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree(); it!= end; ++it){ //count ++; //std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); list_iterator.sort(OcTreeVolumeSortPredicate); list_depr.sort(OcTreeVolumeSortPredicate); std::cout << "All inner lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; // traverse all leaf nodes, timing: gettimeofday(&start, NULL); // start timers count = 0; for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){ // do something: // std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; count++; } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n"; /** * bounding box tests */ //tree->expand(); // test complete tree (should be equal to no bbx) OcTreeKey bbxMinKey, bbxMaxKey; double temp_x,temp_y,temp_z; tree->getMetricMin(temp_x,temp_y,temp_z); octomap::point3d bbxMin(temp_x,temp_y,temp_z); tree->getMetricMax(temp_x,temp_y,temp_z); octomap::point3d bbxMax(temp_x,temp_y,temp_z); EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey)); EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey)); OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey); EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey)); OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx(); EXPECT_TRUE(end_bbx == tree->end_leafs_bbx()); OcTree::leaf_iterator it = tree->begin_leafs(); EXPECT_TRUE(it == tree->begin_leafs()); OcTree::leaf_iterator end = tree->end_leafs(); EXPECT_TRUE(end == tree->end_leafs()); for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){ EXPECT_TRUE(it == it_bbx); } EXPECT_TRUE(it == end && it_bbx == end_bbx); // now test an actual bounding box: tree->expand(); // (currently only works properly for expanded tree (no multires) bbxMin = point3d(-1, -1, - 1); bbxMax = point3d(3, 2, 1); EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey)); EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey)); typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap; KeyVolumeMap bbxVoxels; count = 0; for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx(); it!= end; ++it) { count++; OcTreeKey currentKey = it.getKey(); // leaf is actually a leaf: EXPECT_FALSE(it->hasChildren()); // leaf exists in tree: OcTreeNode* node = tree->search(currentKey); EXPECT_TRUE(node); EXPECT_EQ(node, &(*it)); // all leafs are actually in the bbx: for (unsigned i = 0; i < 3; ++i){ // if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){ // std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i] // << "size: "<< it.getSize()<< std::endl; // } EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]); } bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize())); } EXPECT_EQ(bbxVoxels.size(), count); std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n"; // compare with manual BBX check on all leafs: for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) { OcTreeKey key = it.getKey(); if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0] && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1] && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2]) { KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key); EXPECT_FALSE(bbxIt == bbxVoxels.end()); EXPECT_TRUE(key == bbxIt->first); EXPECT_EQ(it.getSize(), bbxIt->second); } } // test tree with one node: OcTree simpleTree(0.01); simpleTree.updateNode(point3d(10, 10, 10), 5.0f); for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) { std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; } std::cout << "Tests successful\n"; return 0; }
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; }