Rgy.0920.connect

src/ompl/base/PlannerDataStorage.h

@@ -271,7 +271,6 @@ namespace ompl
                         if (!weight_opt.has_value())
                             OMPL_ERROR("Unable to get edge weight");
                         Cost weight {weight_opt.value()};
-                        OMPL_DEBUG("Storing edge (%i to %i) with weight %f", fromVertex, toVertex, weight.value());
                         // Convert to new structure
                         auto edgeData {PlannerDataEdgeData()};
                         edgeData.e_ = &pd.getEdge(fromVertex, toVertex);

src/ompl/base/objectives/src/MinimaxObjective.cpp

@@ -68,8 +68,7 @@ ompl::base::Cost ompl::base::MinimaxObjective::motionCost(const State *s1, const
     Cost lastCost = this->stateCost(s2);
     if (this->isCostBetterThan(worstCost, lastCost))
         worstCost = lastCost;
-
-    return worstCost;
+    return Cost(si_->getStateSpace()->distance(s1, s2) * worstCost.value());
 }
 
 ompl::base::Cost ompl::base::MinimaxObjective::combineCosts(Cost c1, Cost c2) const

src/ompl/tools/multiplan/src/ParallelPlan.cpp

@@ -174,10 +174,10 @@ void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t mi
 
         if (nrSol >= maxSolCount)
             ptc->terminate();
-
         double found_path_length {pdef_->getSolutionPath()->length()};
-        OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf found by %s in %lf seconds", found_path_length, planner->getName().c_str(), duration);
-
+        double found_path_cost {pdef_->getSolutionPath()->cost(pdef_->getOptimizationObjective()).value()};
+        OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf and cost: %lf found by %s in %lf seconds", found_path_length, found_path_cost, planner->getName().c_str(), duration);
+        // OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf found by %s in %lf seconds", found_path_length, planner->getName().c_str(), duration);
         const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
 
         std::lock_guard<std::mutex> slock(phlock_);

src/ompl/tools/thunder/src/SPARSdb.cpp

@@ -200,6 +200,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta
 
     // Start
     OMPL_INFORM("Looking for a node near the problem start");
+    OMPL_INFORM("jajaja");
     if (!findGraphNeighbors(start, startVertexCandidateNeighbors_))
     {
         OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_);
@@ -1330,8 +1331,11 @@ bool ompl::geometric::SPARSdb::findGraphNeighbors(const base::State *state, std:
     base::State *stateCopy = si_->cloneState(state);
 
     if (!radius.has_value()) {
+        OMPL_INFORM("No radius provided, using sparseDelta_");
         radius = sparseDelta_;
     }
+    OMPL_INFORM("radius: %f", *radius);
+    OMPL_INFORM("sparseDelta_: %f", sparseDelta_);
     // Don't check for visibility
     graphNeighborhood.clear();
     stateProperty_[queryVertex_] = stateCopy;