WIP: multiple obstacle sources

MRPT · Jun 28, 2021 · 461045d · 461045d
1 parent 8029686
commit 461045d
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 24 deletions.
diff --git a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp
@@ -174,7 +174,7 @@ void prepare_selfdriving(mvsim::World& world)
         grid->getOccGrid().getAsPointCloud(*obsPts);
     });
 
-    sd.navigator.config_.obstacleSource =
+    sd.navigator.config_.globalMapObstacleSource =
         selfdriving::ObstacleSource::FromStaticPointcloud(obsPts);
 
     // Vehicle interface:

diff --git a/libselfdriving/include/selfdriving/algos/TPS_RRTstar.h b/libselfdriving/include/selfdriving/algos/TPS_RRTstar.h
@@ -153,7 +153,8 @@ class TPS_RRTstar : public mrpt::system::COutputLogger
 
     mrpt::maps::CPointsMap::Ptr cached_local_obstacles(
         const MotionPrimitivesTreeSE2& tree, const TNodeID nodeID,
-        const mrpt::maps::CPointsMap& globalObstacles, double MAX_XY_DIST);
+        const std::vector<mrpt::maps::CPointsMap::Ptr>& globalObstacles,
+        double                                          MAX_XY_DIST);
 
     /** for use in cached_local_obstacles(), local_obstacles_cache_ */
     struct LocalObstaclesInfo

diff --git a/libselfdriving/include/selfdriving/algos/WaypointSequencer.h b/libselfdriving/include/selfdriving/algos/WaypointSequencer.h
@@ -104,7 +104,8 @@ class WaypointSequencer : public mrpt::system::COutputLogger
          *  @{ */
         VehicleMotionInterface::Ptr vehicleMotionInterface;
 
-        ObstacleSource::Ptr obstacleSource;
+        ObstacleSource::Ptr globalMapObstacleSource;
+        ObstacleSource::Ptr localSensedObstacleSource;
 
         TrajectoriesAndRobotShape ptgs;
 

diff --git a/libselfdriving/include/selfdriving/data/PlannerInput.h b/libselfdriving/include/selfdriving/data/PlannerInput.h
@@ -15,10 +15,10 @@ namespace selfdriving
 {
 struct PlannerInput
 {
-    SE2_KinState              stateStart, stateGoal;
-    mrpt::math::TPose2D       worldBboxMin, worldBboxMax;  //!< World bounds
-    ObstacleSource::Ptr       obstacles;
-    TrajectoriesAndRobotShape ptgs;
+    SE2_KinState        stateStart, stateGoal;
+    mrpt::math::TPose2D worldBboxMin, worldBboxMax;  //!< World bounds
+    std::vector<ObstacleSource::Ptr> obstacles;
+    TrajectoriesAndRobotShape        ptgs;
 };
 
 }  // namespace selfdriving
diff --git a/libselfdriving/src/algos/TPS_RRTstar.cpp b/libselfdriving/src/algos/TPS_RRTstar.cpp
@@ -152,7 +152,9 @@ PlannerOutput TPS_RRTstar::plan(const PlannerInput& in)
     const DrawFreePoseParams drawParams(in, tree, searchRadius, goalNodeId);
 
     // obstacles (TODO: dynamic over future time?):
-    const auto obstaclePoints = in.obstacles->obstacles();
+    std::vector<mrpt::maps::CPointsMap::Ptr> obstaclePoints;
+    for (const auto& os : in.obstacles)
+        if (os) obstaclePoints.emplace_back(os->obstacles());
 
     //  3  |  for i \in [1,N] do
     for (size_t rrtIter = 0; rrtIter < params_.maxIterations; rrtIter++)
@@ -194,7 +196,7 @@ PlannerOutput TPS_RRTstar::plan(const PlannerInput& in)
             // if (dist < params_.minStepLength)break;
 
             const auto& localObstacles = cached_local_obstacles(
-                tree, nodeId, *obstaclePoints, MAX_XY_DIST);
+                tree, nodeId, obstaclePoints, MAX_XY_DIST);
 
             const auto&             srcNode = tree.nodes().at(nodeId);
             auto&                   ptg     = *in.ptgs.ptgs.at(ptgIdx);
@@ -883,7 +885,8 @@ TPS_RRTstar::path_to_nodes_list_t TPS_RRTstar::find_reachable_nodes_from(
 
 mrpt::maps::CPointsMap::Ptr TPS_RRTstar::cached_local_obstacles(
     const MotionPrimitivesTreeSE2& tree, const TNodeID nodeID,
-    const mrpt::maps::CPointsMap& globalObstacles, double MAX_XY_DIST)
+    const std::vector<mrpt::maps::CPointsMap::Ptr>& globalObstacles,
+    double                                          MAX_XY_DIST)
 {
     // reuse?
     const auto& node = tree.nodes().at(nodeID);

diff --git a/libselfdriving/src/algos/WaypointSequencer.cpp b/libselfdriving/src/algos/WaypointSequencer.cpp
@@ -28,7 +28,7 @@ void WaypointSequencer::initialize()
 
     // Check that config_ holds all the required fields:
     ASSERT_(config_.vehicleMotionInterface);
-    ASSERT_(config_.obstacleSource);
+    ASSERT_(config_.globalMapObstacleSource);
     ASSERT_(config_.ptgs.initialized());
 
     // sanity check: PTG max distance must be >= than RRT* step lengths:
@@ -396,8 +396,6 @@ waypoint_idx_t WaypointSequencer::find_next_waypoint_for_planner()
 WaypointSequencer::PathPlannerOutput WaypointSequencer::path_planner_function(
     WaypointSequencer::PathPlannerInput ppi)
 {
-    ppi.pi.obstacles = config_.obstacleSource;
-
     const double RRT_BBOX_MARGIN = 4.0;  // [meters]
 
     mrpt::math::TBoundingBoxf bbox;
@@ -439,19 +437,33 @@ WaypointSequencer::PathPlannerOutput WaypointSequencer::path_planner_function(
     // time profiler:
     planner.profiler_.enable(false);
 
-    // Add cost maps:
-    MRPT_TODO("Add the static world cost map");
+    // ~~~~~~~~~~~~~~
+    // Add cost maps
+    // ~~~~~~~~~~~~~~
+
+    // cost map #1: obstacles from current sensors
+    // =============
 #if 0
     {
-        // cost map:
         auto costmap =
             selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
                 *obsPts);
-
         planner.costEvaluators_.push_back(costmap);
     }
 #endif
-    MRPT_TODO("Add the preferred waypoints cost map");
+
+    // cost map #2: prefer to go thru waypoints
+    // =============
+    MRPT_TODO("Add this cost map");
+
+    // ~~~~~~~~~~~~~~~~~~
+    // Obstacles sources
+    // ~~~~~~~~~~~~~~~~~~
+    if (config_.globalMapObstacleSource)
+        ppi.pi.obstacles.push_back(config_.globalMapObstacleSource);
+
+    if (config_.localSensedObstacleSource)
+        ppi.pi.obstacles.push_back(config_.localSensedObstacleSource);
 
     // verbosity level:
     planner.setMinLoggingLevel(this->getMinLoggingLevel());

diff --git a/libselfdriving/src/algos/render_tree.cpp b/libselfdriving/src/algos/render_tree.cpp
@@ -343,15 +343,18 @@ auto selfdriving::render_tree(
     // Obstacles:
     if (ro.draw_obstacles)
     {
-        auto obj = mrpt::opengl::CPointCloud::Create();
+        for (const auto& os : pi.obstacles)
+        {
+            auto obj = mrpt::opengl::CPointCloud::Create();
 
-        const auto obs = pi.obstacles->obstacles();
+            const auto obs = os->obstacles();
 
-        obj->loadFromPointsMap(obs.get());
+            obj->loadFromPointsMap(obs.get());
 
-        obj->setPointSize(ro.point_size_obstacles);
-        obj->setColor_u8(ro.color_obstacles);
-        scene.insert(obj);
+            obj->setPointSize(ro.point_size_obstacles);
+            obj->setColor_u8(ro.color_obstacles);
+            scene.insert(obj);
+        }
     }
 
     // The current set of local obstacles: