Removed PC based on cost-impact

PathPlanning · May 17, 2021 · d27dd05 · d27dd05
1 parent f81bbe5
commit d27dd05
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 10 deletions.
diff --git a/cbs.cpp b/cbs.cpp
@@ -32,7 +32,7 @@ bool CBS::init_root(const Map &map, const Task &task)
             //conflict.path2 = pathB;
             if(pathA.cost > root.paths[conflict.agent1].cost && pathB.cost > root.paths[conflict.agent2].cost)
             {
-                conflict.overcost = std::min(pathA.cost - root.paths[conflict.agent1].cost, pathB.cost - root.paths[conflict.agent2].cost);
+                conflict.overcost = 0;//std::min(pathA.cost - root.paths[conflict.agent1].cost, pathB.cost - root.paths[conflict.agent2].cost);
                 root.cardinal_conflicts.push_back(conflict);
             }
             else if(pathA.cost > root.paths[conflict.agent1].cost || pathB.cost > root.paths[conflict.agent2].cost)
@@ -225,12 +225,13 @@ Conflict CBS::get_conflict(std::list<Conflict> &conflicts)
     auto best_it = conflicts.begin();
     for(auto it = conflicts.begin(); it != conflicts.end(); it++)
     {
-        if(it->overcost > 0)
+        /*if(it->overcost > 0)
         {
             if(best_it->overcost < it->overcost || (fabs(best_it->overcost - it->overcost) < CN_EPSILON && best_it->t < it->t))
                 best_it = it;
         }
-        else if(best_it->t < it->t)
+        else */
+        if(best_it->t < it->t)
             best_it = it;
     }
 
@@ -519,17 +520,17 @@ void CBS::find_new_conflicts(const Map &map, const Task &task, CBS_Node &node, s
             }
             else if (new_pathA.cost < 0)
             {
-                c.overcost = new_pathB.cost - old_cost;
+                c.overcost = 0;//new_pathB.cost - old_cost;
                 cardinal_conflictsA.push_back(c);
             }
             else if (new_pathB.cost < 0)
             {
-                c.overcost = new_pathA.cost - path.cost;
+                c.overcost = 0;//new_pathA.cost - path.cost;
                 cardinal_conflictsA.push_back(c);
             }
             else if(new_pathA.cost > path.cost && new_pathB.cost > old_cost)
             {
-                c.overcost = std::min(new_pathA.cost - path.cost, new_pathB.cost - old_cost);
+                c.overcost = 0;//std::min(new_pathA.cost - path.cost, new_pathB.cost - old_cost);
                 cardinal_conflictsA.push_back(c);
             }
             else if(new_pathA.cost > path.cost || new_pathB.cost > old_cost)
@@ -557,17 +558,17 @@ void CBS::find_new_conflicts(const Map &map, const Task &task, CBS_Node &node, s
             }
             else if (new_pathA.cost < 0)
             {
-                c.overcost = new_pathB.cost - old_cost;
+                c.overcost = 0;//new_pathB.cost - old_cost;
                 cardinal_conflictsA.push_back(c);
             }
             else if (new_pathB.cost < 0)
             {
-                c.overcost = new_pathA.cost - path.cost;
+                c.overcost = 0;//new_pathA.cost - path.cost;
                 cardinal_conflictsA.push_back(c);
             }
             else if(new_pathA.cost > path.cost && new_pathB.cost > old_cost)
             {
-                c.overcost = std::min(new_pathA.cost - path.cost, new_pathB.cost - old_cost);
+                c.overcost = 0;//std::min(new_pathA.cost - path.cost, new_pathB.cost - old_cost);
                 cardinal_conflictsA.push_back(c);
             }
             else if(new_pathA.cost > path.cost || new_pathB.cost > old_cost)

diff --git a/const.h b/const.h
@@ -1,7 +1,7 @@
 #ifndef CONST_H
 #define CONST_H
 
-#define CN_USE_CARDINAL  0 //1 - true, 0 - false
+#define CN_USE_CARDINAL  1 //1 - true, 0 - false
 #define CN_HLH_TYPE      0 // 0 - no hlh, 1 - solve lpp by simplex, 2 - greedly take disjoint conflicts
 #define CN_USE_DS        0 //1 - true, 0 - false
 #define CN_TIMELIMIT     30 // in seconds