Skip to content

cbs_roadmap: support for a different cost (min pathlength) #41

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

cbs_roadmap: support for a different cost (min pathlength) #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 43 additions & 2 deletions example/cbs_roadmap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ struct Vertex {

struct Edge {
std::unordered_set<edge_t> conflictingEdges;
int cost; // use mm here to keep it as an integer
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
Expand Down Expand Up @@ -272,7 +273,7 @@ class Environment {
State n(s.time + 1, v);
if (stateValid(n) && transitionValid(s.time, *eit)) {
neighbors.emplace_back(
Neighbor<State, Action, int>(n, *eit, 1));
Neighbor<State, Action, int>(n, *eit, m_roadmap[*eit].cost));
}
}

Expand Down Expand Up @@ -454,6 +455,29 @@ int main(int argc, char* argv[]) {
}

// read roadmap

// find shortest edge
int shortest_edge_cost = std::numeric_limits<int>::max();
for (const auto& edge : config["roadmap"]["edges"]) {
auto v1str = edge[0].as<std::string>();
auto v2str = edge[1].as<std::string>();
// compute Eucledian cost in mm
float x1 = config["roadmap"]["vertices"][v1str][0].as<float>();
float y1 = config["roadmap"]["vertices"][v1str][1].as<float>();
float x2 = config["roadmap"]["vertices"][v2str][0].as<float>();
float y2 = config["roadmap"]["vertices"][v2str][1].as<float>();
float dx = (x2 - x1);
float dy = (y2 - y1);
float dist = sqrtf(dx * dx + dy * dy);
int dist_in_mm = dist * 1000;
if (dist_in_mm != 0 and dist_in_mm < shortest_edge_cost) {
shortest_edge_cost = dist_in_mm;
}
}

const int self_loop_cost = shortest_edge_cost; // waiting is as costly as traversing the shortest edge in the graph
const int base_cost = 0; // no fixed cost per timestep

roadmap_t roadmap;
std::unordered_map<std::string, vertex_t> vertexMap;

Expand Down Expand Up @@ -481,10 +505,26 @@ int main(int argc, char* argv[]) {
vertexMap.insert(std::make_pair(v2str, v2));
roadmap[v2].name = v2str;
}
// add the edge
auto e1 = boost::add_edge(v1, v2, roadmap);
// compute Eucledian cost in mm
float x1 = config["roadmap"]["vertices"][v1str][0].as<float>();
float y1 = config["roadmap"]["vertices"][v1str][1].as<float>();
float x2 = config["roadmap"]["vertices"][v2str][0].as<float>();
float y2 = config["roadmap"]["vertices"][v2str][1].as<float>();
float dx = (x2 - x1);
float dy = (y2 - y1);
float dist = sqrtf(dx * dx + dy * dy);
int dist_in_mm = dist * 1000;
if (dist_in_mm == 0) {
dist_in_mm = self_loop_cost;
}

roadmap[e1.first].cost = dist_in_mm + base_cost;
edgeVec.push_back(e1.first);
if (config["roadmap"]["undirected"].as<bool>()) {
auto e2 = boost::add_edge(v2, v1, roadmap);
roadmap[e2.first].cost = dist_in_mm + base_cost;
edgeVec.push_back(e2.first);
roadmap[e1.first].conflictingEdges.insert(e2.first);
roadmap[e2.first].conflictingEdges.insert(e1.first);
Expand All @@ -494,6 +534,7 @@ int main(int argc, char* argv[]) {
if (config["roadmap"]["allow_wait_actions"].as<bool>()) {
for (const auto& v : vertexMap) {
auto e = boost::add_edge(v.second, v.second, roadmap);
roadmap[e.first].cost = self_loop_cost + base_cost;
edgeVec.push_back(e.first);
}
}
Expand Down Expand Up @@ -559,7 +600,7 @@ int main(int argc, char* argv[]) {
out << " agent" << a << ":" << std::endl;
for (const auto& state : solution[a].states) {
out << " - v: " << roadmap[state.first.vertex].name << std::endl
<< " t: " << state.second << std::endl;
<< " cost: " << state.second << std::endl;
}
}
} else {
Expand Down
82 changes: 82 additions & 0 deletions test/issue40.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
{agents: [{goal: '14', name: agent0, start: '13'}, {goal: '10', name: agent1, start: '1'},
{goal: '5', name: agent2, start: '6'}], roadmap: {allow_wait_actions: false, conflicts: [
[1, 2, 3, 4, 5, 16, 23, 41, 46, 63, 64, 82, 87], [0, 2, 3, 4, 5, 31, 42, 46,
70, 71, 72, 82, 90], [0, 1, 3, 4, 5, 22, 24, 35, 43, 46, 75, 76, 82, 92],
[0, 1, 2, 4, 5, 29, 38, 44, 46, 79, 82, 95], [0, 1, 2, 3, 5, 25, 30, 34, 39,
45, 46, 80, 82, 96], [0, 1, 2, 3, 4, 41, 42, 43, 44, 45, 46, 82], [7, 8, 9,
15, 47, 50, 51, 52, 53, 54, 55, 56, 83, 84], [6, 8, 9, 10, 18, 48, 50, 57,
58, 59, 83, 85], [6, 7, 9, 17, 36, 49, 50, 77, 83, 93], [6, 7, 8, 47, 48,
49, 50, 83], [7, 11, 12, 13, 14, 15, 18, 47, 51, 56, 57, 58, 59, 84, 85],
[10, 12, 13, 14, 15, 21, 47, 52, 56, 60, 61, 62, 84, 86], [10, 11, 13, 14, 15,
26, 47, 53, 56, 65, 66, 67, 84, 88], [10, 11, 12, 14, 15, 28, 47, 54, 56,
68, 69, 84, 89], [10, 11, 12, 13, 15, 20, 27, 40, 47, 55, 56, 81, 84, 97],
[6, 10, 11, 12, 13, 14, 47, 51, 52, 53, 54, 55, 56, 84], [0, 17, 18, 23, 48,
51, 57, 59, 63, 64, 85, 87], [8, 16, 18, 36, 48, 51, 58, 59, 77, 85, 93],
[7, 10, 16, 17, 48, 51, 57, 58, 59, 85], [20, 21, 32, 37, 52, 60, 62, 78, 86,
94], [14, 19, 21, 27, 40, 52, 61, 62, 81, 86, 97], [11, 19, 20, 52, 60, 61,
62, 86], [2, 23, 24, 35, 41, 57, 63, 64, 75, 76, 87, 92], [0, 16, 22, 41,
57, 63, 64, 87], [2, 22, 25, 26, 35, 53, 65, 67, 75, 76, 88, 92], [4, 24,
26, 30, 34, 39, 53, 66, 67, 80, 88, 96], [12, 24, 25, 53, 65, 66, 67, 88],
[14, 20, 28, 40, 54, 68, 69, 81, 89, 97], [13, 27, 54, 68, 69, 89], [3, 30,
31, 38, 42, 70, 72, 79, 90, 95], [4, 25, 29, 31, 34, 39, 42, 71, 72, 80, 90,
96], [1, 29, 30, 42, 70, 71, 72, 90], [19, 33, 37, 73, 74, 78, 91, 94], [
32, 73, 74, 91], [4, 25, 30, 35, 39, 43, 63, 65, 75, 76, 80, 92, 96], [2,
22, 24, 34, 43, 63, 65, 75, 76, 92], [8, 17, 49, 58, 77, 93], [19, 32, 60,
73, 78, 94], [3, 29, 44, 70, 79, 95], [4, 25, 30, 34, 45, 66, 71, 75, 80,
96], [14, 20, 27, 55, 61, 68, 81, 97], [0, 5, 22, 23, 42, 43, 44, 45, 46,
57, 64, 82, 87], [1, 5, 29, 30, 31, 41, 43, 44, 45, 46, 72, 82, 90], [2, 5,
34, 35, 41, 42, 44, 45, 46, 63, 65, 76, 82, 92], [3, 5, 38, 41, 42, 43, 45,
46, 70, 79, 82, 95], [4, 5, 39, 41, 42, 43, 44, 46, 66, 71, 75, 80, 82, 96],
[0, 1, 2, 3, 4, 5, 41, 42, 43, 44, 45, 82], [6, 9, 10, 11, 12, 13, 14, 15, 48,
49, 50, 56, 83, 84], [7, 9, 16, 17, 18, 47, 49, 50, 51, 59, 83, 85], [8, 9,
36, 47, 48, 50, 58, 77, 83, 93], [6, 7, 8, 9, 47, 48, 49, 83], [6, 10, 15,
16, 17, 18, 48, 52, 53, 54, 55, 56, 59, 84, 85], [6, 11, 15, 19, 20, 21, 51,
53, 54, 55, 56, 62, 84, 86], [6, 12, 15, 24, 25, 26, 51, 52, 54, 55, 56, 67,
84, 88], [6, 13, 15, 27, 28, 51, 52, 53, 55, 56, 69, 84, 89], [6, 14, 15,
40, 51, 52, 53, 54, 56, 61, 68, 81, 84, 97], [6, 10, 11, 12, 13, 14, 15, 47,
51, 52, 53, 54, 55, 84], [7, 10, 16, 18, 22, 23, 41, 58, 59, 64, 85, 87],
[7, 10, 17, 18, 36, 49, 57, 59, 77, 85, 93], [7, 10, 16, 17, 18, 48, 51, 57,
58, 85], [11, 19, 21, 37, 61, 62, 73, 78, 86, 94], [11, 20, 21, 40, 55, 60,
62, 68, 81, 86, 97], [11, 19, 20, 21, 52, 60, 61, 86], [0, 16, 22, 23, 34,
35, 43, 64, 65, 76, 87, 92], [0, 16, 22, 23, 41, 57, 63, 87], [12, 24, 26,
34, 35, 43, 63, 66, 67, 76, 88, 92], [12, 25, 26, 39, 45, 65, 67, 71, 75,
80, 88, 96], [12, 24, 25, 26, 53, 65, 66, 88], [13, 27, 28, 40, 55, 61, 69,
81, 89, 97], [13, 27, 28, 54, 68, 89], [1, 29, 31, 38, 44, 71, 72, 79, 90,
95], [1, 30, 31, 39, 45, 66, 70, 72, 75, 80, 90, 96], [1, 29, 30, 31, 42,
70, 71, 90], [32, 33, 37, 60, 74, 78, 91, 94], [32, 33, 73, 91], [2, 22, 24,
34, 35, 39, 45, 66, 71, 76, 80, 92, 96], [2, 22, 24, 34, 35, 43, 63, 65, 75,
92], [8, 17, 36, 49, 58, 93], [19, 32, 37, 60, 73, 94], [3, 29, 38, 44, 70,
95], [4, 25, 30, 34, 39, 45, 66, 71, 75, 96], [14, 20, 27, 40, 55, 61, 68,
97], [0, 1, 2, 3, 4, 5, 41, 42, 43, 44, 45, 46], [6, 7, 8, 9, 47, 48, 49,
50], [6, 10, 11, 12, 13, 14, 15, 47, 51, 52, 53, 54, 55, 56], [7, 10, 16,
17, 18, 48, 51, 57, 58, 59], [11, 19, 20, 21, 52, 60, 61, 62], [0, 16, 22,
23, 41, 57, 63, 64], [12, 24, 25, 26, 53, 65, 66, 67], [13, 27, 28, 54, 68,
69], [1, 29, 30, 31, 42, 70, 71, 72], [32, 33, 73, 74], [2, 22, 24, 34, 35,
43, 63, 65, 75, 76], [8, 17, 36, 49, 58, 77], [19, 32, 37, 60, 73, 78], [
3, 29, 38, 44, 70, 79], [4, 25, 30, 34, 39, 45, 66, 71, 75, 80], [14, 20,
27, 40, 55, 61, 68, 81]], edges: [['0', '5'], ['0', '8'], ['0', '10'], ['0',
'13'], ['0', '14'], ['0', '0'], ['1', '2'], ['1', '3'], ['1', '11'], ['1',
'1'], ['2', '3'], ['2', '4'], ['2', '6'], ['2', '7'], ['2', '15'], ['2', '2'],
['3', '5'], ['3', '11'], ['3', '3'], ['4', '12'], ['4', '15'], ['4', '4'], [
'5', '10'], ['5', '5'], ['6', '10'], ['6', '14'], ['6', '6'], ['7', '15'],
['7', '7'], ['8', '13'], ['8', '14'], ['8', '8'], ['9', '12'], ['9', '9'], [
'10', '14'], ['10', '10'], ['11', '11'], ['12', '12'], ['13', '13'], ['14',
'14'], ['15', '15'], ['5', '0'], ['8', '0'], ['10', '0'], ['13', '0'], ['14',
'0'], ['0', '0'], ['2', '1'], ['3', '1'], ['11', '1'], ['1', '1'], ['3', '2'],
['4', '2'], ['6', '2'], ['7', '2'], ['15', '2'], ['2', '2'], ['5', '3'], ['11',
'3'], ['3', '3'], ['12', '4'], ['15', '4'], ['4', '4'], ['10', '5'], ['5',
'5'], ['10', '6'], ['14', '6'], ['6', '6'], ['15', '7'], ['7', '7'], ['13',
'8'], ['14', '8'], ['8', '8'], ['12', '9'], ['9', '9'], ['14', '10'], ['10',
'10'], ['11', '11'], ['12', '12'], ['13', '13'], ['14', '14'], ['15', '15'],
['0', '0'], ['1', '1'], ['2', '2'], ['3', '3'], ['4', '4'], ['5', '5'], ['6',
'6'], ['7', '7'], ['8', '8'], ['9', '9'], ['10', '10'], ['11', '11'], ['12',
'12'], ['13', '13'], ['14', '14'], ['15', '15']], undirected: false, vertices: {
'0': [0.756037712097168, 0.41884613037109375], '1': [0.2569187581539154, 0.5132027268409729],
'10': [0.6819828152656555, 0.47409766912460327], '11': [0.10270211845636368,
0.43217167258262634], '12': [0.612196147441864, 0.9110383987426758], '13': [
0.8633358478546143, 0.2610264718532562], '14': [0.8063616752624512, 0.5468763709068298],
'15': [0.4008060097694397, 0.8267107605934143], '2': [0.4069139361381531, 0.781825602054596],
'3': [0.30531173944473267, 0.4746146500110626], '4': [0.581429123878479, 0.9061399102210999],
'5': [0.5055835247039795, 0.2836513817310333], '6': [0.7540021538734436, 0.6163807511329651],
'7': [0.25219032168388367, 0.9079001545906067], '8': [0.9002949595451355, 0.3120931088924408],
'9': [0.7279759049415588, 0.8969451785087585]}}}