bug fixes

MRPT · Aug 2, 2022 · 968bc22 · 968bc22
1 parent 143fc56
commit 968bc22
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 10 deletions.
diff --git a/libselfdriving/src/algos/WaypointSequencer.cpp b/libselfdriving/src/algos/WaypointSequencer.cpp
@@ -430,6 +430,10 @@ WaypointSequencer::PathPlannerOutput WaypointSequencer::path_planner_function(
         << ppi.pi.worldBboxMin.asString() << " - "
         << ppi.pi.worldBboxMax.asString());
 
+    MRPT_LOG_DEBUG_STREAM(
+        "[path_planner_function] Using VehicleInterface class: "
+        << config_.vehicleMotionInterface->GetRuntimeClass()->className);
+
     // Do the path planning :
     selfdriving::TPS_Astar planner;
 
@@ -445,18 +449,26 @@ WaypointSequencer::PathPlannerOutput WaypointSequencer::path_planner_function(
     // TODO: Make static list instead of recreating each time?
     planner.costEvaluators_.clear();
 
-    if (auto obs = config_.globalMapObstacleSource->obstacles(); !obs->empty())
+    if (config_.globalMapObstacleSource)
     {
-        planner.costEvaluators_.push_back(
-            selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
-                *obs, config_.globalCostMapParameters));
+        if (auto obs = config_.globalMapObstacleSource->obstacles();
+            obs && !obs->empty())
+        {
+            planner.costEvaluators_.push_back(
+                selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
+                    *obs, config_.globalCostMapParameters));
+        }
     }
-    if (auto obs = config_.localSensedObstacleSource->obstacles();
-        !obs->empty())
+
+    if (config_.localSensedObstacleSource)
     {
-        planner.costEvaluators_.push_back(
-            selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
-                *obs, config_.localCostMapParameters));
+        if (auto obs = config_.localSensedObstacleSource->obstacles();
+            obs && !obs->empty())
+        {
+            planner.costEvaluators_.push_back(
+                selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
+                    *obs, config_.localCostMapParameters));
+        }
     }
 
     // cost map #2: prefer to go thru waypoints

diff --git a/libselfdriving/src/data/Waypoints.cpp b/libselfdriving/src/data/Waypoints.cpp
@@ -89,7 +89,7 @@ Waypoint Waypoint::FromYAML(const mrpt::containers::yaml& d)
         s.asSequence()[0].as<double>(), s.asSequence()[1].as<double>()};
 
     if (d.has("targetHeading"))
-        wp.targetHeading = d["targetHeading"].as<bool>();
+        wp.targetHeading = d["targetHeading"].as<double>();
 
     wp.targetFrameId   = d["targetFrameId"].as<std::string>();
     wp.allowedDistance = d["allowedDistance"].as<double>();