reaches_goal changes dynoplan

src/db_cbs.cpp

@@ -182,6 +182,14 @@ int main(int argc, char* argv[]) {
         options_tdbastar.max_motions = std::min<size_t>(options_tdbastar.max_motions, 1e6);
       }
       // disable/enable motions 
+      // for (auto& iter : robot_motions) {
+      //     for (size_t i = 0; i < problem.robotTypes.size(); ++i) {
+      //         if (iter.first == problem.robotTypes[i]) {
+      //             disable_motions(robots[i], options_tdbastar.delta, filter_duplicates, alpha, options_tdbastar.max_motions, iter.second);
+      //             break;
+      //         }
+      //     }
+      // }
       bool solved_db = false;
       HighLevelNode start;
       start.solution.resize(env["robots"].size());
@@ -244,11 +252,10 @@ int main(int argc, char* argv[]) {
           HighLevelNode newNode = P;
           size_t tmp_robot_id = c.first;
           newNode.id = id;
-#ifdef DBG_PRINTS
+// #ifdef DBG_PRINTS
           std::cout << "Node ID is " << id << std::endl;
-#endif
+// #endif
           newNode.constraints[tmp_robot_id].insert(newNode.constraints[tmp_robot_id].end(), c.second.begin(), c.second.end());
-          std::cout << "New node constraints size: " << newNode.constraints.size() << std::endl;
           newNode.cost -= newNode.solution[tmp_robot_id].trajectory.cost;
 #ifdef DBG_PRINTS
           std::cout << "New node cost: " << newNode.cost << std::endl;

src/dbcbs_utils.hpp

@@ -139,4 +139,58 @@ void export_solutions(const std::vector<LowLevelPlan<dynobench::Trajectory>>& so
 
         }
     }
-}
+}
+
+void disable_motions(
+  std::shared_ptr<dynobench::Model_robot>& robot,
+  float delta,
+  bool filterDuplicates,
+  float alpha,
+  size_t num_max_motions,
+  std::vector<dynoplan::Motion>& motions) {
+    ompl::NearestNeighbors<dynoplan::Motion *> *T_m;
+    // enable all motions
+    for (size_t i = 0; i < motions.size(); ++i) {
+      motions[i].disabled = false;
+    } 
+    if(filterDuplicates){
+      size_t num_duplicates = 0;
+      dynoplan::Motion fakeMotion;
+      fakeMotion.idx = -1;
+      fakeMotion.traj.states.push_back(Eigen::VectorXd(robot->nx));
+      std::vector<dynoplan::Motion *> neighbors_m;
+      for (const auto& m : motions) {
+        if (m.disabled) {
+          continue;
+        }
+        // fakeMotion.traj.states.at(0) = m.states[0];
+        fakeMotion.states[0] = m.states[0];
+        T_m->nearestR(&fakeMotion, delta*alpha, neighbors_m);
+        for (dynoplan::Motion* nm : neighbors_m){
+          if (nm == &m || nm->disabled) { 
+            continue;
+          }
+          float goal_delta = robot->distance(m.traj.states.back(), nm->traj.states.back());
+          if (goal_delta < delta*(1-alpha)) {
+          nm->disabled = true;
+          ++num_duplicates;
+          }
+        }
+      }
+      std::cout << "There are " << num_duplicates << " duplicate motions!" << std::endl;
+
+    }
+    // limit to num_max_motions
+    size_t num_enabled_motions = 0;
+    for (size_t i = 0; i < motions.size(); ++i){
+      if (!motions[i].disabled) {
+        if (num_enabled_motions >= num_max_motions) {
+          motions[i].disabled = true;
+        } else {
+          ++num_enabled_motions;
+        }
+      }
+    }
+    std::cout << "There are " << num_enabled_motions << " motions enabled." << std::endl;
+
+  }