diff --git a/example/cbs_roadmap.cpp b/example/cbs_roadmap.cpp index df60d95b..91bbaa6e 100644 --- a/example/cbs_roadmap.cpp +++ b/example/cbs_roadmap.cpp @@ -42,6 +42,7 @@ struct Vertex { struct Edge { std::unordered_set conflictingEdges; + int cost; // use mm here to keep it as an integer }; typedef boost::adjacency_list(n, *eit, 1)); + Neighbor(n, *eit, m_roadmap[*eit].cost)); } } @@ -454,6 +455,29 @@ int main(int argc, char* argv[]) { } // read roadmap + + // find shortest edge + int shortest_edge_cost = std::numeric_limits::max(); + for (const auto& edge : config["roadmap"]["edges"]) { + auto v1str = edge[0].as(); + auto v2str = edge[1].as(); + // compute Eucledian cost in mm + float x1 = config["roadmap"]["vertices"][v1str][0].as(); + float y1 = config["roadmap"]["vertices"][v1str][1].as(); + float x2 = config["roadmap"]["vertices"][v2str][0].as(); + float y2 = config["roadmap"]["vertices"][v2str][1].as(); + 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 vertexMap; @@ -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 y1 = config["roadmap"]["vertices"][v1str][1].as(); + float x2 = config["roadmap"]["vertices"][v2str][0].as(); + float y2 = config["roadmap"]["vertices"][v2str][1].as(); + 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()) { 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); @@ -494,6 +534,7 @@ int main(int argc, char* argv[]) { if (config["roadmap"]["allow_wait_actions"].as()) { 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); } } @@ -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 { diff --git a/test/issue40.yaml b/test/issue40.yaml new file mode 100644 index 00000000..effeaf20 --- /dev/null +++ b/test/issue40.yaml @@ -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]}}} \ No newline at end of file