//*****************************************************
// Here are the functions that actually matters
void Graph::DijkstrasTest()
{
addEdge("", "", 0);
cout << "This program will tell you the shortest path between 2 cities. " << endl;
Node* start = getStart(nodes);
while(!start)
{
cout << "Not a valid starting point, please re-enter: " << endl;
start = getStart(nodes);
}
Node* destination = getDestination(nodes);
while (!destination)
{
cout << "Not a valid destination, please re-enter: " << endl;
destination = getDestination(nodes);
}
start->distanceFromStart = 0; // set start node
Dijkstras(edges, nodes);
printShortestRouteTo(destination, start);
}
void DijkstrasTest()
{
Node* n1 = new Node('1', 15, 13);
Node* n2 = new Node('2', 15, 7);
Node* n3 = new Node('3', 33, 7);
Node* n4 = new Node('4', 32.8, 16);
Node* n5 = new Node('5', 6, 13);
//Node* f = new Node('6', 0, 0);
//Node* g = new Node('7', 0, 0);
Edge* e1 = new Edge(n1, n2, 1); //NEED to reset the distance of the edge
Edge* e2 = new Edge(n2, n3, 2);
Edge* e3 = new Edge(n3, n4, 2);
Edge* e4 = new Edge(n4, n5, 1);
/*Edge* e5 = new Edge(b, f, 3);
Edge* e6 = new Edge(c, e, 3);
Edge* e7 = new Edge(e, f, 2);
Edge* e8 = new Edge(d, g, 1);
Edge* e9 = new Edge(g, f, 1);*/
n1->distanceFromStart = 0; // set start node
Dijkstras();
PrintLoadShortestRouteTo(n4);
// TODO: Node / Edge memory cleanup not included
}
bool gen_path(path_generator::generatePath::Request &req, path_generator::generatePath::Response &res)
{
path_navigator::Waypoints *w = NULL;
if(req.type == 0) //generate dikstra's path
{
w = new path_navigator::Waypoints();
Dijkstras(gmap, req.init_pos, req.dst_pos, w);
}
else if(req.type == 1)
{
w = new path_navigator::Waypoints();
RandomWalk(gmap, req.init_pos, w, 10); //default: perform 10 steps before you stop
}
res.w = *w;
return true;
}
// Johnson's
std::vector<std::vector<std::vector<std::pair<unsigned long, double>>>>
Algorithms::Johnsons(Graph& graph){
auto graphx = std::vector<std::vector<std::pair<unsigned long, double>>>();
auto num = graph.get_num_vertices();
auto vert = std::vector<std::pair<unsigned long, double>>();
for(auto& lis: graph.list){
graphx.push_back(lis);
}
for(int i = 0; i< num; i++){
vert.push_back(std::pair<unsigned long, double>(i,0));
}
graphx.push_back(vert);
auto tempg = Graph(graphx);
unsigned long start_index = num;
num = num + 1;
std::vector<double> distance = std::vector<double>(num);
std::vector<unsigned long> predecessor = std::vector<unsigned long>(num);
//initalize values
for(int i = 0; i< num; i++) {
distance[i] = std::numeric_limits<double>::infinity();
predecessor[i] = 0; //
}
distance[start_index] = 0;
//compute optimal path
for(int index = 0; index < num; index++) {
for(int i = 0; i< num; i++) {
std::vector<std::pair<unsigned long, double>> row = tempg.list[i];
for(auto& pair: row) {
if(distance[i] + pair.second < distance[pair.first]) {
distance[pair.first] = distance[i] + pair.second;
predecessor[pair.first] = i;
}
}
}
}
//check for negative cycles
for(int i = 0; i < num; i++){
std::vector<std::pair<unsigned long, double>> rows = tempg.list[i];
for(auto& pairs: rows) {
if(distance[i] + pairs.second < distance[pairs.first]) {
throw Algorithms_Exception("Contains Negative Cycles");
}
}
}
auto h = std::vector<double>(num);
//compute h[] distances
h[num-1] = 0;
for(int i = 0 ; i < num-1 ; i++){
std::vector<std::pair<unsigned long, double>> path;
unsigned long current = i;
path.push_back(std::pair<unsigned long, double>(current, distance[current]));
do {
current = predecessor[current];
auto newpair = std::pair<unsigned long, double>(current, distance[current]);
path.push_back(newpair);
} while(current != num-1);
std::reverse(path.begin(), path.end());
double total = 0;
for(auto& edge: path){
total += edge.second;
}
h[i] = total;
}
//construct graph with with new weights
auto newgraph = graph.list;
for(int i = 0 ; i< newgraph.size(); i++){
auto temp = newgraph[i];
auto newedges = std::vector<std::pair<unsigned long, double>>();
for(int j = 0; j < temp.size(); j++){
std::pair<unsigned long, double> temppair = temp[j];
double value = (temppair.second + h[i] - h[temppair.first]);
newedges.push_back(std::pair<unsigned long, double>(temppair.first,value));
}
newgraph[i] = newedges;
}
auto finalgraph = Graph(newgraph);
auto length = finalgraph.get_num_vertices();
auto final_weight = std::vector<std::vector<std::vector<std::pair<unsigned
long, double>>>>(length);
for(int i = 0; i < length ; i++){
final_weight[i] = std::vector<std::vector<std::pair<unsigned long,
double>>>(length);
}
//compute paths between all nodes
for(int i = 0; i< length; i++){
for(int j = 0 ; j< length; j++){
if(i == j){
auto tem = std::vector<std::pair<unsigned long, double>>();
tem.push_back(std::pair<unsigned long, double>(0,0));
final_weight[i][j] = tem;
}else if(i>j){
auto xyz = final_weight[j][i];
std::reverse(xyz.begin(), xyz.end());
final_weight[i][j] = xyz;
}else{
try{
final_weight[i][j] = Dijkstras(finalgraph,i,j);
}catch(Algorithms_Exception){
final_weight[i][j] = std::vector<std::pair<unsigned long, double>>();
}
}
}
}
return final_weight;
}
void reach_middle_point(list< boost::shared_ptr<position_tracker::Position> > nn_pos_list)
{
if(nn_pos_list.size() < 2) //need to have 2 neighbors for the function to work
{
cout << "I have " << nn_pos_list.size() << " neighbors. I need 2." << endl;
return;
}
ros::spinOnce(); //to get the most recent position of the robot
//(1) find the nodes that are closest to the position of the neighbors
loadMap();
vector<Node *> closest_nodes;
list< boost::shared_ptr<position_tracker::Position> >::iterator itl;
int cnt = 0;
for(itl = nn_pos_list.begin(); itl != nn_pos_list.end(); ++itl)
{
position_tracker::Position *pin = new position_tracker::Position();
pin->x = (*itl)->x;
pin->y = (*itl)->y;
pin->theta = (*itl)->theta;
std::cout << "Robot " << cnt << " in position (" << pin->x << ", " << pin->y << ", " << pin->theta << ")" << std::endl;
//find the closest node np to the input position p
double min_dist = 10000;
int min_ind = -1;
vector<Node *>::iterator itn;
for(itn = nodes.begin(); itn != nodes.end(); ++itn)
{
double dist = sqrt(pow(pin->x - (*itn)->p->x, 2) + pow(pin->y - (*itn)->p->y, 2));
if(dist < min_dist)
{
min_dist = dist;
min_ind = itn - nodes.begin();
}
}
closest_nodes.insert(closest_nodes.end(), nodes[min_ind]);
std::cout << "It's closest position is (" << nodes[min_ind]->p->x << ", " << nodes[min_ind]->p->y << ")" << std::endl;
cnt++;
}
//(2)For now: assume 2 neighbors, find the position in between the 2 neighbors
//shortest path between the 2 neighbors
//vector<Node *> closest_nodes_vec;
//std::copy(closest_nodes.begin(), closest_nodes.end(), closest_nodes_vec.begin());
//closest_nodes_vec[0]->distanceFromStart = 0; // set start node
closest_nodes[0]->distanceFromStart = 0; // set start node
Dijkstras(); //NEED TO DO THAT ONLY ONCE IN THE BEGINNING, NOT ALL THE TIME!!!!!!!!
/*vector<Node *>::iterator itw;
for(itw = waypoints.begin(); itw != waypoints.end(); ++itw)
{
cout << "waypint id = " << (*itw)->id << endl;
} */
waypoints.clear();
PrintLoadShortestRouteTo(closest_nodes[1]); //stores the path in the waypoints vector =>
//PrintLoadShortestRouteTo(closest_nodes_vec[1]); //stores the path in the waypoints vector => NEED TO CHANGE THAT (I want to get the path back as a vector and then do whatever I want with that)
//BUT, for now...
//get position among 2 neighbors
Node *middle_init = waypoints[ceil(waypoints.size()/2)];
std::cout << "The position in the middle of the robots is (" << middle_init->p->x << ", " << middle_init->p->y << ")" << std::endl;
//(3)Find the closest node nc to my current position
loadMap(); //!!!!!!!!!!!!!!!
vector<Node *>::iterator itn2;
cout << "All the nodes: " << endl;
for(itn2 = nodes.begin(); itn2 != nodes.end(); ++itn2)
{
cout << "node: id = " << (*itn2)->id << ", x = " << (*itn2)->p->x << ", y = " << (*itn2)->p->y << endl;
}
//find the middle node using the newly loaded map
double min_dist = 10000;
int min_ind = -1;
vector<Node *>::iterator itn;
for(itn = nodes.begin(); itn != nodes.end(); ++itn)
{
double dist = sqrt(pow(middle_init->p->x - (*itn)->p->x, 2) + pow(middle_init->p->y - (*itn)->p->y, 2));
if(dist < min_dist)
{
min_dist = dist;
min_ind = itn - nodes.begin();
}
}
Node *middle = nodes[min_ind];
//find closest node to the current position of the robot
min_dist = 10000;
min_ind = -1;
//vector<Node *>::iterator itn;
for(itn = nodes.begin(); itn != nodes.end(); ++itn)
{
double dist = sqrt(pow(cur_pos.x - (*itn)->p->x, 2) + pow(cur_pos.y - (*itn)->p->y, 2));
if(dist < min_dist)
{
min_dist = dist;
min_ind = itn - nodes.begin();
}
}
Node *nc = nodes[min_ind];
std::cout << "My closest position is (" << nc->p->x << ", " << nc->p->y << ")" << std::endl;
//(4)Do shortest path between the current position of the robot and the position in between its 2 neighbors
nc->distanceFromStart = 0; // set start node
Dijkstras();
waypoints.clear();
PrintLoadShortestRouteTo(middle); //THIS TIME, I want the generated path to be stored in the waypoints list
//set your goal to be 3 nodes backwards from and follow the way to the goal (make your waypoints contain all the nodes up to the 3 nodes away from the destination)
/*waypoints.pop_back();
waypoints.pop_back();
waypoints.pop_back();
//loadWaypoints(); //NEED TO CHANGE THAT !!!!*/
//======================= don't move for now =============================
//EXECUTE THE PATH (probably need to move it somewhere else)
vector<Node *>::iterator it;
for(it = waypoints.begin(); it != waypoints.end(); ++it) //reach every single waypoint generated by the planner
{
goal_pos_x = (*it)->p->x;
goal_pos_y = (*it)->p->y;
//move to the next waypoint
while(sqrt(pow(goal_pos_x - cur_pos.x, 2) + pow(goal_pos_y - cur_pos.y, 2)) > Tdist)
{
ros::spinOnce();
drive();
}//end while(current goal is not reached)
}//end while(1)*/
}
