From 461045d6528a3f90abab6f61e247b5940aa40604 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 28 Jun 2021 17:54:55 +0200 Subject: [PATCH] WIP: multiple obstacle sources --- .../selfdriving-simulator-gui.cpp | 2 +- .../include/selfdriving/algos/TPS_RRTstar.h | 3 +- .../selfdriving/algos/WaypointSequencer.h | 3 +- .../include/selfdriving/data/PlannerInput.h | 8 +++--- libselfdriving/src/algos/TPS_RRTstar.cpp | 9 ++++-- .../src/algos/WaypointSequencer.cpp | 28 +++++++++++++------ libselfdriving/src/algos/render_tree.cpp | 15 ++++++---- 7 files changed, 44 insertions(+), 24 deletions(-) diff --git a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp index a482518..2909e77 100644 --- 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 index 9d417e5..242d3a8 100644 --- 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& 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 index ae77abd..1722818 100644 --- 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 index faf2853..40b3dc9 100644 --- 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 obstacles; + TrajectoriesAndRobotShape ptgs; }; } // namespace selfdriving diff --git a/libselfdriving/src/algos/TPS_RRTstar.cpp b/libselfdriving/src/algos/TPS_RRTstar.cpp index a7f29c3..c4bff20 100644 --- 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 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& 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 index dae2e76..da79064 100644 --- 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 index bd93bbf..f0f5436 100644 --- 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: