MPS/greedy_planner2.cpp at master · dressel/MPS · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#include "greedy_planner2.h"

using std::endl;


GreedyPlanner2::GreedyPlanner2(string paramfile, string logpath)
	:MyPlanner(paramfile, logpath) {}

int GreedyPlanner2::initialize()
{
	/* Create the logging file */
	if (start_log())
		return -1;

	string path = read_config(_param_file);
	if (path == "error")
		return -1;

	/* check that the filter is of  the correct type */
	if (this->filter->type != 0)
	{
		planner_log << "Greedy2 expects a discrete filter." << endl;
		return -1;
	}
	DF * f = static_cast<DF *>(this->filter);
	_n = f->n;
	_cell_size = f->cell_size;

	return 0;
}


/**
 * Loops through all possible actions, selecting the best.
 */
vector<float> GreedyPlanner2::get_action()
{
	DF *f = static_cast<DF *>(filter);
	if (f->obs_probs.size() > 0)
		return get_action_fast();
	return get_action_slow();
}

vector<float> GreedyPlanner2::get_action_slow()
{
	int xi, yi;
	vector<double> xp;
	double mi, best_mi, best_x, best_y, half_cell;
	best_mi = -99999.0;
	half_cell = _cell_size / 2;
	xp.resize(3);
	xp[2] = 0;


	for (xi = 0; xi < _n; xi++)
	{
		xp[0] = xi * _cell_size + half_cell;
		for (yi = 0; yi < _n; yi++)
		{
			xp[1] = yi * _cell_size + half_cell;
			mi = filter->mutual_information(_uav, xp);
			if (mi > best_mi)
			{
				best_mi = mi;
				best_x = xp[0];
				best_y = xp[1];
			}
		}
	}

	double dx = best_x - _uav.x;
	double dy = best_y - _uav.y;
	vector<float> command;
	command.resize(3);
	command[0] = dy;
	command[1] = dx;
	command[2] = 0;

	return command;
}

vector<float> GreedyPlanner2::get_action_fast()
{
	int xv, yv;
	double mi, best_mi, best_x, best_y;
	best_mi = -99999.0;


	for (xv = 0; xv < _n; xv++)
	{
		for (yv = 0; yv < _n; yv++)
		{
			mi = filter->mutual_information(_uav, xv, yv);
			if (mi > best_mi)
			{
				best_mi = mi;
				best_x = xv*_cell_size;
				best_y = yv*_cell_size;
			}
		}
	}

	double dx = best_x - _uav.x;
	double dy = best_y - _uav.y;
	vector<float> command;
	command.resize(3);
	command[0] = dy;
	command[1] = dx;
	command[2] = 0;

	return command;
}