From ff9bfd77aa56407127584f4da4732effc53d324d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 20 Dec 2022 14:56:22 +0100 Subject: [PATCH] first nav events; cleaner and safer code --- apps/path-planner-cli/path-planner-cli.cpp | 8 +- .../include/selfdriving/algos/NavEngine.h | 5 +- .../selfdriving/data/MotionPrimitivesTree.h | 2 +- .../include/selfdriving/data/PlannerOutput.h | 14 +-- .../selfdriving/data/ProgressCallbackData.h | 4 +- .../interfaces/VehicleMotionInterface.h | 20 ++-- libselfdriving/src/algos/NavEngine.cpp | 95 +++++++++++++++---- libselfdriving/src/algos/TPS_Astar.cpp | 7 +- 8 files changed, 114 insertions(+), 41 deletions(-) diff --git a/apps/path-planner-cli/path-planner-cli.cpp b/apps/path-planner-cli/path-planner-cli.cpp index bb86f89..beed976 100644 --- a/apps/path-planner-cli/path-planner-cli.cpp +++ b/apps/path-planner-cli/path-planner-cli.cpp @@ -324,9 +324,15 @@ static void do_plan_path() << " overall edges, " << plan.motionTree.nodes().size() << " nodes\n"; + if (!plan.bestNodeId.has_value()) + { + std::cerr << "No bestNodeId in plan output.\n"; + return; + } + // backtrack: auto [plannedPath, pathEdges] = - plan.motionTree.backtrack_path(plan.bestNodeId); + plan.motionTree.backtrack_path(*plan.bestNodeId); if (!arg_noRefine.isSet()) { diff --git a/libselfdriving/include/selfdriving/algos/NavEngine.h b/libselfdriving/include/selfdriving/algos/NavEngine.h index ed39960..a0826d8 100644 --- a/libselfdriving/include/selfdriving/algos/NavEngine.h +++ b/libselfdriving/include/selfdriving/algos/NavEngine.h @@ -275,7 +275,8 @@ class NavEngine : public mrpt::system::COutputLogger /** Update the GUI with a partial or final path only (no whole tree) */ void send_path_to_viz_and_navlog( - const MotionPrimitivesTreeSE2& tree, const TNodeID finalNode, + const MotionPrimitivesTreeSE2& tree, + const std::optional& finalNode, const PlannerInput& originalPlanInput, const std::vector& costEvaluators); @@ -536,6 +537,8 @@ class NavEngine : public mrpt::system::COutputLogger void merge_new_plan_if_better(const PathPlannerOutput& result); + void internal_mark_current_wp_as_reached(); + struct AboutToReachWpInfo { AboutToReachWpInfo() = default; diff --git a/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h b/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h index e55793b..2cd367a 100644 --- a/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h +++ b/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h @@ -20,12 +20,12 @@ #include #include #include +#include #include #include namespace selfdriving { -using mrpt::graphs::INVALID_NODEID; using mrpt::graphs::TNodeID; /** Generic base for metrics */ diff --git a/libselfdriving/include/selfdriving/data/PlannerOutput.h b/libselfdriving/include/selfdriving/data/PlannerOutput.h index 14485b3..3d70d5d 100644 --- a/libselfdriving/include/selfdriving/data/PlannerOutput.h +++ b/libselfdriving/include/selfdriving/data/PlannerOutput.h @@ -10,6 +10,7 @@ #include #include +#include #include namespace selfdriving @@ -37,14 +38,15 @@ struct PlannerOutput double pathCost = std::numeric_limits::max(); /** The tree node ID for the goal pose */ - TNodeID goalNodeId = mrpt::graphs::INVALID_NODEID; + std::optional goalNodeId; - /** The tree node with the smallest cost-to-goal. This will be identical - * to goalNodeId for successful plans reaching the desired goal state, or - * something different for unfinished or unsuccessful plans. + /** The tree node with the smallest cost-to-goal. This will be + * identical to goalNodeId for successful plans reaching the desired + * goal state, or something different for unfinished or unsuccessful + * plans. */ - TNodeID bestNodeId = mrpt::graphs::INVALID_NODEID; - cost_t bestNodeIdCostToGoal = std::numeric_limits::max(); + std::optional bestNodeId; + cost_t bestNodeIdCostToGoal = std::numeric_limits::max(); /** The generated motion tree that explores free space starting at "start" */ diff --git a/libselfdriving/include/selfdriving/data/ProgressCallbackData.h b/libselfdriving/include/selfdriving/data/ProgressCallbackData.h index 219cb66..184c458 100644 --- a/libselfdriving/include/selfdriving/data/ProgressCallbackData.h +++ b/libselfdriving/include/selfdriving/data/ProgressCallbackData.h @@ -24,7 +24,7 @@ struct ProgressCallbackData cost_t bestCostFromStart = std::numeric_limits::max(); cost_t bestCostToGoal = std::numeric_limits::max(); MotionPrimitivesTreeSE2::edge_sequence_t bestPath; - TNodeID bestFinalNode = INVALID_NODEID; + std::optional bestFinalNode; const MotionPrimitivesTreeSE2* tree = nullptr; const PlannerInput* originalPlanInput = nullptr; const std::vector* costEvaluators = nullptr; @@ -33,4 +33,4 @@ struct ProgressCallbackData using planner_progress_callback_t = std::function; -} // namespace selfdriving \ No newline at end of file +} // namespace selfdriving diff --git a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h b/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h index 7d86814..bb8e752 100644 --- a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h +++ b/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h @@ -192,10 +192,10 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger, MRPT_LOG_WARN("Default on_nav_start() event handler called."); } /** - * @brief Callback if navigation ended by an accepted trigger or reached the last - * specified waypoint + * @brief Callback if navigation ended by an accepted trigger or reached the + * last specified waypoint */ - virtual void on_nav_end() + virtual void on_nav_end() { MRPT_LOG_WARN("Default on_nav_end() event handler called."); } @@ -215,16 +215,18 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger, MRPT_LOG_WARN("Default on_apparent_collision() event handler called."); } /** - * @brief Callback upon reaching a waypoint in a waypoint sequeunce. + * @brief Callback upon reaching a waypoint in a waypoint sequeunce. * Mostly used for logging * @param waypoint_index index of waypoint in the sequence - * @param reached_skipped bool indicator if waypoint was reached or skipped + * @param reached_skipped whether it was reached(true) or skipped (false) */ - virtual void on_waypoint_reached(const size_t waypoint_index, - [[maybe_unused]]bool reached_skipped) + virtual void on_waypoint_reached( + const size_t waypoint_index, bool reached_skipped) { - MRPT_LOG_WARN(mrpt::format("Default on_waypoint_reached() index = %zu event handler called.", - waypoint_index).c_str()); + MRPT_LOG_WARN_FMT( + "Default on_waypoint_reached() index = %zu event " + "handler called (event='%s').", + waypoint_index, reached_skipped ? "reached" : "skipped"); } /** * @brief Callback when NavEngine cannot reach a specified target location diff --git a/libselfdriving/src/algos/NavEngine.cpp b/libselfdriving/src/algos/NavEngine.cpp index 93b8848..20ca326 100644 --- a/libselfdriving/src/algos/NavEngine.cpp +++ b/libselfdriving/src/algos/NavEngine.cpp @@ -917,7 +917,7 @@ void NavEngine::check_new_planner_output() _.active_plan_reset(); auto [path, edges] = _.activePlanOutput.po.motionTree.backtrack_path( - _.activePlanOutput.po.bestNodeId); + *_.activePlanOutput.po.bestNodeId); // Correct PTG arguments according to the final actual poses. // Needed to correct for lattice approximations: @@ -1342,7 +1342,8 @@ void NavEngine::send_planner_output_to_viz(const PathPlannerOutput& ppo) } void NavEngine::send_path_to_viz_and_navlog( - const MotionPrimitivesTreeSE2& tree, const TNodeID finalNode, + const MotionPrimitivesTreeSE2& tree, + const std::optional& finalNode, const PlannerInput& originalPlanInput, const std::vector& costEvaluators) { @@ -1487,7 +1488,8 @@ void NavEngine::send_current_state_to_viz_and_navlog() } if (const auto& triggOdom = _.lastEnqueuedTriggerOdometry; - triggOdom.has_value() && !_.activePlanPath.empty()) + triggOdom.has_value() && !_.activePlanPath.empty() && + _.activePlanInitOdometry.has_value()) { auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.15); glCorner->setPose( @@ -1757,28 +1759,35 @@ bool NavEngine::approach_target_controller() const auto out = config_.targetApproachController->execute(tacIn); MRPT_LOG_INFO_FMT( - "Controller towards target executed since atrw.aboutToReach==true, " + "Controller towards target executed since atrw. aboutToReach=true, " "results: targetDist=%f handled=%s reachedDetected=%s", atrw.distanceToWaypoint, out.handled ? "YES" : "NO", out.reachedDetected ? "YES" : "NO"); - MRPT_TODO("Check out.reachedDetected"); - if (out.handled) { - ASSERT_(config_.vehicleMotionInterface); - if (out.generatedMotion) + if (out.reachedDetected) { - config_.vehicleMotionInterface->motion_execute( - out.generatedMotion, std::nullopt); - - // log record copy: - _.sentOutCmdInThisIteration = out.generatedMotion; + // end of navigation to waypoint: + internal_mark_current_wp_as_reached(); } else { - config_.vehicleMotionInterface->motion_execute( - std::nullopt, std::nullopt); + // It's a motion command: + ASSERT_(config_.vehicleMotionInterface); + if (out.generatedMotion) + { + config_.vehicleMotionInterface->motion_execute( + out.generatedMotion, std::nullopt); + + // log record copy: + _.sentOutCmdInThisIteration = out.generatedMotion; + } + else + { + config_.vehicleMotionInterface->motion_execute( + std::nullopt, std::nullopt); + } } } @@ -1844,8 +1853,15 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result) const double currentPlanDistToGoal = (goal - _.activePlanPath.rbegin()->pose).translation().norm(); + if (!result.po.bestNodeId.has_value()) + { + // error: no good plan. + MRPT_LOG_WARN("Dropping new path planning result, no bestNodeID."); + return; + } + const double newPlanDistToGoal = - (goal - result.po.motionTree.nodes().at(result.po.bestNodeId).pose) + (goal - result.po.motionTree.nodes().at(*result.po.bestNodeId).pose) .translation() .norm(); @@ -1866,7 +1882,7 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result) // for-the-future segment that was just received: auto [newPath, newEdges] = - result.po.motionTree.backtrack_path(result.po.bestNodeId); + result.po.motionTree.backtrack_path(*result.po.bestNodeId); // Correct PTG arguments according to the final actual poses. // Needed to correct for lattice approximations: @@ -1924,3 +1940,48 @@ void NavEngine::merge_new_plan_if_better(const PathPlannerOutput& result) _.activePlanEdgesSentOut.insert(i); _.activePlanInitOdometry = formerEdgeInitOdometry; } + +void NavEngine::internal_mark_current_wp_as_reached() +{ + auto& _ = innerState_; + + // sanity checks: + ASSERT_(_.pathPlannerTargetWpIdx.has_value()); + ASSERT_LT_(*_.pathPlannerTargetWpIdx, _.waypointNavStatus.waypoints.size()); + + const waypoint_idx_t reachedIdx = *_.pathPlannerTargetWpIdx; + + // We are about to mark "reachedIdx" as reached. + // First, go over the former ones, since the last "reached" and mark them as + // skipped: + { + waypoint_idx_t lastReached = reachedIdx; + while (lastReached > 0 && + !_.waypointNavStatus.waypoints.at(lastReached).reached) + { // go back: + lastReached--; + } + // now, mark all in the range [lastReached+1, ..., reachedIdx-1] as + // skipped: + for (waypoint_idx_t i = lastReached + 1; i + 1 <= reachedIdx; i++) + { + // mark as skipped: + _.waypointNavStatus.waypoints.at(i).skipped = true; + + // user callbacks: + config_.vehicleMotionInterface->on_waypoint_reached( + i, false /* =skipped */); + } + } + + // mark final one as reached: + _.waypointNavStatus.waypoints.at(reachedIdx).reached = true; + + // user callbacks: + config_.vehicleMotionInterface->on_waypoint_reached( + reachedIdx, true /* =reached*/); + + // clear statuses so we can launch a new plan in the next iteration: + _.pathPlannerTargetWpIdx.reset(); + _.active_plan_reset(); +} diff --git a/libselfdriving/src/algos/TPS_Astar.cpp b/libselfdriving/src/algos/TPS_Astar.cpp index 59e159f..3e747b2 100644 --- a/libselfdriving/src/algos/TPS_Astar.cpp +++ b/libselfdriving/src/algos/TPS_Astar.cpp @@ -424,11 +424,11 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in) tLastCallback = tNow; const auto [foundPath, pathEdges] = - tree.backtrack_path(po.bestNodeId); + tree.backtrack_path(*po.bestNodeId); // call user callback: ProgressCallbackData pcd; - pcd.bestCostFromStart = tree.nodes().at(po.bestNodeId).cost_; + pcd.bestCostFromStart = tree.nodes().at(*po.bestNodeId).cost_; pcd.bestCostToGoal = po.bestNodeIdCostToGoal; pcd.bestFinalNode = po.bestNodeId; pcd.bestPath = std::move(pathEdges); @@ -455,8 +455,7 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in) #endif po.success = po.goalNodeId == po.bestNodeId; - if (po.bestNodeId != INVALID_NODEID) - po.pathCost = tree.nodes().at(po.bestNodeId).cost_; + if (po.bestNodeId) po.pathCost = tree.nodes().at(*po.bestNodeId).cost_; po.computationTime = mrpt::Clock::nowDouble() - planInitTime;