whoenig / libmultirobotplanning Goto Github PK

View Code? Open in Web Editor NEW

766.0 766.0 210.0 3.64 MB

Library with search algorithms for task and path planning for multi robot/agent systems

License: MIT License

CMake 1.64% C++ 83.65% Python 14.65% Shell 0.05%

libmultirobotplanning's People

Contributors

Stargazers

Watchers

Forkers

nlimpert mylinyuzhi aandreychuk rchaaaaa kylevedder serenewalden jason-zhangyp kongan sitebots-com sugie-liu zhangaigh sichitong atb033 sohamsankaran donsimp ttalaeik daluoozi sweetquiet ddlyrabbit nishant-codex qmqmyu aqueffelec huaxu-qiu bangyanz supertigim shushman forrest-z li9616 shevchenco123 xuehait wenjiebit collector-m ct2034 randvoorhies you2996 leechangyo lijuncheng001 aiharney wayne-xixi jimmy91826 fei-xia hityunhongxu sushant-chavan vikikkdi dmchen2015 qwerty35 imcabbage itwpub richard-he ut-amrl jian-dong lekoon zhng1456 lijian8 malintha omgalex mieximiemie chen116 lewlovehow hydra5 gitsper nykabhishek zijinoier daydreamer2023 drordayan chunting wang-zheng-jiu axina mmehrez jjqcat podshara jinqiang yxk0423 raj-khare hxdaze sfrobot yangfuyuan yuhud te-takumiyajima leac123 agcxgz321 riven521 shike1239 cleverpuppy chengc1 w470062742 fshi2006 bkauto hustyangjia triplez23 wikan2019 hepingchao jim-uav albaintelligence lyk125 hamad-am emmanuel555 gaows123 codesystemmanage jianyujiang

libmultirobotplanning's Issues

In a specific map scene, the cbs algorithm cannot find a solution and falls into an infinite loop

I once made a map as attachment,
'#' means obstacle,
'.' means space,
'1','2','3' is start position of three agent, and 'A', 'B', 'C' is the goal position one by one.

When I apply the cbs implemention, It falls into an infinite loop.
To reproduce the problem, I slightly modifed the source code of cbs.cpp as following.
......
// std::unordered_set obstacles;
// for (const auto& node : config["map"]["obstacles"]) {
//obstacles.insert(Location(node[0].as(), node[1].as()));
// }
std::unordered_set obstacles;
std::ifstream map(mapFile);

int y = 0;
while (map.good()) {
std::string line;
std::getline(map, line);
int x = 0;
for (char c : line) {
if (c == '#') {
obstacles.insert(Location(x, y));
}
++x;
}
++y;
}
......

map.txt

Potential Bug in ECBS

Hello,

First of all, thank you for this awesome repository! These implementations will help a lot of people.

I just cloned the repository and I ran my own trivial examples. However, I ran into some problems in two examples (see attached files). I use the below command to run the test files in the build directory.

./ecbs -i test1.yaml -o output.yaml -w 1.3

The program runs for a while and eventually terminates displaying the below error.

libc++abi.dylib: terminating with uncaught exception of type std::length_error: vector

The funny thing with the first test case (test1.txt) is that the program is able to find a solution, if I change the starting point of Agent1 from 4,2 to 8,2. When Agent1 starts on 4,2, it has to go back to make way for Agent0. Test2 is a bit more complicated scenario, but the error is the same.

test1.txt
test2.txt

Edit: Accidentally posted this a little earlier than I intended :)

Thank you for your time and great work!

Emre

Target Assignment Part

Hi,

In your code, you are using successive_shortest_path_nonnegative_weights to get the solution for Target Assignment part.

libMultiRobotPlanning/include/libMultiRobotPlanning/assignment.hpp

Line 84 in b01a527

successive_shortest_path_nonnegative_weights(

But Successive Shortest Path algorithm is Pseudo-polynomial time, why not just use hungarian algorithm which is O(n^3)?

Missing packages in InstallPackagesUbuntu

Hello,

I am really thankful for this incredible repository. But to me there were two packages missing that I had to download to give it a proper run.

doxygen
graphviz

Maybe you can add this quick fix in your file :)

Again thanks a lot!

Leon

Low-level search in CBS-TA

Hi,
After reading the paper for CBS-TA, I had the understanding that CBS-TA uses A* search for low-level search (while resolving conflicts) (i.e. during execution of CBS part). But on going through the code, it seems that CBS-TA uses Dijkstra's algorithm for both the 1st assignment, and all subsequent assignments. Please let me know if I am missing something. I had started a thread on the discussion section, but it seemed that it was not sending notifications, so used this forum.

Error while running the roadmap example in the README

Firstly thanks a lot for releasing such high-quality code. Kindly, I have question about errors I am facing while running roadmap example in the README.

After running python3 ../tools/annotate_roadmap.py ../test/mapf_simple1_roadmap_to_annotate.yaml mapf_simple1_roadmap_annotated.yaml I am facing the following output

Namespace(map='../test/mapf_simple1_roadmap_to_annotate.yaml', out='mapf_simple1_roadmap_annotated.yaml', radius=0.3)
Traceback (most recent call last):
  File "/home/ssfc/libMultiRobotPlanning/build/../tools/annotate_roadmap.py", line 106, in <module>
    main()
  File "/home/ssfc/libMultiRobotPlanning/build/../tools/annotate_roadmap.py", line 32, in main
    roadmap = add_edge_conflicts(args.radius, roadmap)
              ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/ssfc/libMultiRobotPlanning/build/../tools/annotate_roadmap.py", line 39, in add_edge_conflicts
    conflicts = compute_edge_conflicts(radius, roadmap)
                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/ssfc/libMultiRobotPlanning/build/../tools/annotate_roadmap.py", line 89, in compute_edge_conflicts
    if collision.precheck_bounding_box(E, p0, p1, q0, q1):
       ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/ssfc/libMultiRobotPlanning/tools/collision.py", line 21, in precheck_bounding_box
    box_p += np.stack([-np.diagonal(E), np.diagonal(E)])
numpy.core._exceptions._UFuncOutputCastingError: Cannot cast ufunc 'add' output from dtype('float64') to dtype('int64') with casting rule 'same_kind'

Any recommendations to overcome this error is highly appreciated

Built for Crazyflie? :D

Hi @whoenig

I've been working on Crazyflies for quite a while, digging for reference code for Crazyflie and realised you have a lot of other useful repos for such as this repo (libMultiRobotPlanning ) and uav_trajectories

Are they some how built for Crazyflies in mind?

Regards
Nickson

Question about fmin in ECBS

Pardon me if this is extremely specific, but I really want to get to the bottom of one thing in ECBS. According to the original paper by Barer et al, in the subsection on Enhanced CBS just before Experimental Results, fmin(i) is the minimal f-value in the OPEN list used for the ith agent's low-level search. Certainly, that should be a lower bound on the optimal consistent cost-to-go.

My understanding is that fmin(i) should be the lowest f-value encountered during the entire low-level focal search for that agent, i.e. the lowest f-value among all of the focal heap entries that were expanded.

According to this line, however (https://github.com/whoenig/libMultiRobotPlanning/blob/master/include/libMultiRobotPlanning/a_star_epsilon.hpp#L210), it seems that the solution.fmin looks at the lowest f-value when the solution is expanded. Is that not different from the lowest f-value seen throughout the search?

Multi-goal TA

Does this support multi-goal task allocation? That is, one robot visiting multiple goal locations?

CBS on roadmap finds suboptimal solution

This is the solution of CBS:

This is a better (shorter) solution:

yaml input (after annotation):

{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]}}}

leads to this output:

statistics:
  success: 1
  cost: 7
  makespan: 3
  runtime: 0.00090839
  highLevelExpanded: 2
  lowLevelExpanded: 95
schedule:
  agent0:
    - v: 13
      t: 0
    - v: 0
      t: 1
    - v: 14
      t: 2
  agent1:
    - v: 1
      t: 0
    - v: 2
      t: 1
    - v: 6
      t: 2
    - v: 10
      t: 3
  agent2:
    - v: 6
      t: 0
    - v: 10
      t: 1
    - v: 5
      t: 2

The shorter solution sends agent0 to v: 8 at t: 1

Paper question

Thanks for this interesting developed search algorithms library.
I'd like to kindly ask if there's a paper that describes the developed ECBS, and the difference with the CBS. Also, if a summary about the difference between ECBS and CBS can be provided that would be highly appreciated

Inter-agent collisions using Prioritized Planning using SIPP

Hello,

I was trying out the package for multi-agent motion planning using SIPP but noticed the presence of collisions when trying to visualize the ouptut.

I tried test_mapf_prioritized_sipp.py with createVideo=True and got "COLLISION! (agent-agent) (...)" messages.
One of the test cases that caused collisions is: test_mapfSimple1. Prioritized Planning using SIPP doesn't seem to account for collisions with the other obstacles.

There may be infinite loop if there is no way from start point to goal point

libMultiRobotPlanning/include/libMultiRobotPlanning/a_star.hpp

Line 85 in b01a527

while (!openSet.empty()) {

I think it could have infinite loop if no way from start point to goal point since there is an additional parameter(time) in State. And when you add a State to the set and queue, timestep may be different for same (x,y) so the set won't have any effort.

Correct me if I'm wrong.

high level of LB in ECBS do not used?

The paper ECBS said that the node that pushed into the FOCAL from OPEN should meet the condition LB<n.cost<=LB*W, but in your code

    bestCost = open.top().cost;

if (newNode.cost <= bestCost * m_w) {
focal.push(handle);
}

should it be bestCost = open.top().LB?
can you explain it? Than you very much.

Assertion triggering on simple crossing scenario

On this scenario

{agents: [{goal: [0, 1], name: agent0, start: [3, 2]}, {goal: [0, 1], name: agent1,
      start: [3, 2]}, {goal: [1, 3], name: agent2, start: [2, 0]}], map: {dimensions: [
      4, 4], obstacles: []}}

calling with ecbs -i in.yaml -o out.yaml -w 1.5 I get this output:

ecbs: /... /libMultiRobotPlanning/include/libMultiRobotPlanning/ecbs.hpp:258: bool libMultiRobotPlanning::ECBS<State, Action, Cost, Conflict, Constraints, Environment>::search(const std::vector<T>&, std::vector<libMultiRobotPlanning::PlanResult<State, Action, Cost> >&) [with State = State; Action = Action; Cost = int; Conflict = Conflict; Constraints = Constraints; Environment = Environment]: Assertion `!newNode.constraints[i].overlap(c.second)' failed.

Tested on d3cfb64

Need help working with cbs_ta and ecbs_ta

Hello! Thank you so much for your repo, very useful information. I was trying to visualize an ecbs_ta and cbs_ta example however I kept running into this:
Traceback (most recent call last): File "visualize.py", line 162, in <module> animation = Animation(map, schedule) File "visualize.py", line 57, in __init__ self.patches.append(Rectangle((d["goal"][0] - 0.25, d["goal"][1] - 0.25), 0.5, 0.5, facecolor=Colors[i%len(Colors)], edgecolor='black', alpha=0.5)) KeyError: 'goal'

I was using the mapfta_simple1_a1.yaml example test file, and I know the key in this file is potentialGoals. So I am not sure how I can visualize my results when I am using either task allocation algorithm??

I would really appreciate the help, thank you in advance!

Potential Bug on ECBS-TA

Hi, I am currently studying MAPF topics. This awesome repo helps me a lot! I am interested in (E)CBS-TA, it is very impressive work.

When I tested ECBS-TA with unlabeled MAPF settings, I found that several agents seemed to collide. Could you check them?

Environment

OSX 10.15.7, cmake version 3.17.3, with the latest master repo

Example

case 1

instance: random-32-32-20_30agents_1.yaml
output: output_random-32-32-20_30agents_1.yaml
execution

./ecbs_ta -o output_random-32-32-20_30agents_1.yaml -i ../benchmark/random-32-32-20_30agents_1.yaml -w 1.05
python3 ../example/visualize.py ../benchmark/random-32-32-20_30agents_1.yaml output_random-32-32-20_30agents_1.yaml

collision: agent2 and agent17
The log of agent17 seems to be strange. It uses the same timestep (t=0) several times, e.g., the log starts with (x=26, y=15, t=0), (x=26, y=15, t=0) [...].

case 2

instance: random-32-32-20_30agents_2.yaml
output: output_random-32-32-20_30agents_2.yaml

./ecbs_ta -o output_random-32-32-20_30agents_2.yaml -i ../benchmark/random-32-32-20_30agents_2.yaml -w 1.05
python3 ../example/visualize.py ../benchmark/random-32-32-20_30agents_2.yaml output_random-32-32-20_30agents_2.yaml

collision: agent22 and agent28, agent9 and agent28
The log of agent28 uses t=7 several times.

Thank you in advance for your time.

Kei

Code questions

Hello,
I'm very interested in MAPF. I want to ask you, what editor does this code run in? This problem occurs when I run CBS. CPP in the vscode editor. I found a lot of solutions in Google, but I can't solve them. I hope I can get some suggestions from you.

Maybe a bug in CBS?

When I reading overlap function in Constraints struct from cbs.cpp, I encounter this:

std::unordered_set<VertexConstraint> vertexConstraints;
std::unordered_set<EdgeConstraint> edgeConstraints; 
bool overlap(const Constraints& other) {
    std::vector<VertexConstraint> vertexIntersection;
    std::vector<EdgeConstraint> edgeIntersection;
    std::set_intersection(vertexConstraints.begin(), vertexConstraints.end(),
                          other.vertexConstraints.begin(),
                          other.vertexConstraints.end(),
                          std::back_inserter(vertexIntersection));
    std::set_intersection(edgeConstraints.begin(), edgeConstraints.end(),
                          other.edgeConstraints.begin(),
                          other.edgeConstraints.end(),
                          std::back_inserter(edgeIntersection));
    return !vertexIntersection.empty() || !edgeIntersection.empty();
  }

As I know, std::set_intersection can only work on ordered set like std::set. The function may not work properly on std::unordered_set.
https://stackoverflow.com/questions/48158811/c-library-method-for-intersection-of-two-unordered-set

Hope for your reply:)

Improve Low-Level Heursistic for CBS and ECBS

Currently, just Manhattan distance is used. Switch to a perfect heuristic, as done in (E)CBS-TA.

Error while running the test and the example available in the README

Thanks for the script provided. Kindly, I have two questions about errors I am facing while running the provided examples in the README.

After running python3 ../test/test_next_best_assignment.py TestNextBestAssignment.test_1by2 I am facing the following output

E
======================================================================
ERROR: test_1by2 (__main__.TestNextBestAssignment)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "../test/test_next_best_assignment.py", line 28, in test_1by2
    r = self.runNextBestAssignment(mapping)
  File "../test/test_next_best_assignment.py", line 15, in runNextBestAssignment
    check=True)
  File "/usr/lib/python3.6/subprocess.py", line 423, in run
    with Popen(*popenargs, **kwargs) as process:
  File "/usr/lib/python3.6/subprocess.py", line 729, in __init__
    restore_signals, start_new_session)
  File "/usr/lib/python3.6/subprocess.py", line 1364, in _execute_child
    raise child_exception_type(errno_num, err_msg, err_filename)
FileNotFoundError: [Errno 2] No such file or directory: './next_best_assignment': './next_best_assignment'

----------------------------------------------------------------------
Ran 1 test in 0.010s

FAILED (errors=1)

After running python3 ../example/visualize.py ../benchmark/32x32_obst204/map_32by32_obst204_agents10_ex1.yaml output.yaml I am facing the following output

../example/visualize.py:163: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
  map = yaml.load(map_file)
Traceback (most recent call last):
  File "../example/visualize.py", line 165, in <module>
    with open(args.schedule) as states_file:
FileNotFoundError: [Errno 2] No such file or directory: 'output.yaml'

Any recommendations to overcome these errors is highly appreciated

Agents look like ignoring obstacles.

Hi.
Firstly thanks a lot for releasing such high-quality code. I bet it is and will be super helpful for many people.

I think I found a problem, though: When I run python3 ../example/visualize.py ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml output.yaml the agents seem to go over obstacles:

Or do I misunderstand something here?
Thanks,
Christian

Undefined References from yaml-cpp During Build Process

Hello,

I am trying to build the demo's using the process outlined in the README on Ubuntu 18.04. The following is an outline of my process:

Install dependencies outlined in the repo
sudo apt-get install g++ cmake libboost-program-options-dev libyaml-cpp-dev clang-tidy clang-format python3-matplotlib graphviz doxygen
mkdir build
cd build
cmake ..
make

CMAKE claims to find all required libraries. However, during the build process, I get an error while building the planners. I have attached the error message below. I would appreciate some guidance on how to resolve this error. Thank you in advance

!

[Question] Comparator for HighLevelNode::cost should be < ?

Beginner's question about this line: https://github.com/whoenig/libMultiRobotPlanning/blob/master/include/libMultiRobotPlanning/cbs.hpp#L189

If I understood the paper correctly, the high-level search is best-first according to lowest node cost; and I believe the d_ary_heap is a min-heap, so shouldn't the line be return cost < n.cost instead? What am I missing?

Assertion failing on special scenarios

For example on this scenario:

    gridmap = np.array([
        [0, 0, 1, 1],
        [0, 0, 0, 1],
        [0, 0, 0, 0],
        [0, 0, 0, 1]
    ])
    starts = [[1, 2],
              [3, 2],
              [2, 1]]
    goals = [[3, 0],
             [2, 2],
             [0, 0]]

or as yaml ...

{agents: [{goal: [3, 0], name: agent0, start: [1, 2]}, {goal: [2, 2], name: agent1,
      start: [3, 2]}, {goal: [0, 0], name: agent2, start: [2, 1]}], map: {dimensions: [
      4, 4], obstacles: [[0, 2], [0, 3], [1, 3], [3, 3]]}}

When I run it with disappear_at_goal this assertion fails:

include/libMultiRobotPlanning/ecbs.hpp:258: bool libMultiRobotPlanning::ECBS<State, Action, Cost, Conflict, Constraints, Environment>::search(const std::vector<T>&, std::vector<libMultiRobotPlanning::PlanResult<State, Action, Cost> >&) [with State = State; Action = Action; Cost = int; Conflict = Conflict; Constraints = Constraints; Environment = Environment]: Assertion `!newNode.constraints[i].overlap(c.second)' failed.

Note: The scenario solves fine when disappear_at_goal is not set.

Error while adding a new action

Hello!
Thanks for sharing your work!
I am trying to add a new action into the example/ECBS.cpp such that in addition to the available action set (up, down, left, and right). The agent can navigate to corner. The modification ended with successful path planning for most of the examples in the benchmark/. However, there are some yamls in the benchmark/ that show a segmentation fault, similar to below.

user:~/libMultiRobotPlanning-master/build$ ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3
Found conflict: 8: Vertex(20,10)
create child with id 1
  success. cost: 2305
create child with id 2
  success. cost: 2303
Found conflict: 8: Edge(20,11,19,11)
create child with id 3
  success. cost: 2305
create child with id 4
  success. cost: 2307
Found conflict: 11: Vertex(21,12)
create child with id 5
  success. cost: 2309
create child with id 6
Segmentation fault (core dumped)

It's important to keep in mind that all the cases showing a segmentation fault, while using the modified example/ECBS.cpp, e.g. ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3 could successfully generate a path by running them via valgrind e.g. valgrind ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3
Below I am providing the modified ecbs.cpp as a .txt
ecbs_corner.txt
Sharing any idea supports treating the shown fault would be highly appreciated.

Map generation script

Hello! Is there a script that generates different agent and map configurations in the YAML format? If so, could you please direct me to where I can find it?

Timeout of cbs_roadmap

On this (unsolvable) scenario, the cbs_roadmap planner times out for me (or takes more than 60s)
Note that vertex 4 is disconnected from the rest of the graph:

{agents: [{goal: '4', name: agent0, start: '0'}], roadmap: {allow_wait_actions: true,
    edges: [['0', '1'], ['0', '3'], ['0', '2'], ['1', '2'], ['2', '3']], undirected: true,
    vertices: {'0': [0.0, 0.0], '1': [1.0, 0.0], '2': [1.0, 1.0], '3': [0.0, 1.0],
      '4': [2.0, 2.0]}}}

and annotated:

agents:
- goal: '4'
  name: agent0
  start: '0'
roadmap:
  allow_wait_actions: false
  conflicts:
  - - 1
    - 2
    - 5
    - 8
    - 10
    - 11
  - - 0
    - 2
    - 4
    - 6
    - 10
    - 13
  - - 0
    - 1
    - 3
    - 7
    - 9
    - 10
    - 12
  - - 2
    - 5
    - 8
    - 9
    - 11
    - 12
  - - 1
    - 7
    - 8
    - 9
    - 12
    - 13
  - - 0
    - 3
    - 6
    - 7
    - 10
    - 11
  - - 1
    - 5
    - 7
    - 9
    - 10
    - 13
  - - 2
    - 4
    - 5
    - 6
    - 8
    - 10
    - 12
  - - 0
    - 3
    - 4
    - 7
    - 11
    - 12
  - - 2
    - 3
    - 4
    - 6
    - 12
    - 13
  - - 0
    - 1
    - 2
    - 5
    - 6
    - 7
  - - 0
    - 3
    - 5
    - 8
  - - 2
    - 3
    - 4
    - 7
    - 8
    - 9
  - - 1
    - 4
    - 6
    - 9
  - []
  edges:
  - - '0'
    - '1'
  - - '0'
    - '3'
  - - '0'
    - '2'
  - - '1'
    - '2'
  - - '2'
    - '3'
  - - '1'
    - '0'
  - - '3'
    - '0'
  - - '2'
    - '0'
  - - '2'
    - '1'
  - - '3'
    - '2'
  - - '0'
    - '0'
  - - '1'
    - '1'
  - - '2'
    - '2'
  - - '3'
    - '3'
  - - '4'
    - '4'
  undirected: false
  vertices:
    '0':
    - 0.0
    - 0.0
    '1':
    - 1.0
    - 0.0
    '2':
    - 1.0
    - 1.0
    '3':
    - 0.0
    - 1.0
    '4':
    - 2.0
    - 2.0

The configured Radius was .2
detected in a0ac3bb

whoenig / libmultirobotplanning Goto Github PK

libmultirobotplanning's People

Contributors

Stargazers

Watchers

Forkers

libmultirobotplanning's Issues

Environment

Example

case 1

case 2

Recommend Projects

Recommend Topics

Recommend Org