diff --git a/libselfdriving/include/selfdriving/algos/WaypointSequencer.h b/libselfdriving/include/selfdriving/algos/WaypointSequencer.h index f0e7407..9148ee3 100644 --- a/libselfdriving/include/selfdriving/algos/WaypointSequencer.h +++ b/libselfdriving/include/selfdriving/algos/WaypointSequencer.h @@ -6,11 +6,16 @@ #pragma once -#include +#include +#include +#include #include #include #include +#include +#include + namespace selfdriving { /** The different states for the navigation system. */ @@ -66,12 +71,16 @@ struct NavErrorReason * } * \enddot * + * All methods are thread-safe, in the sense that mutexes are internally used + * to ensure no undefined navigation state is possible if invoking an object of + * this class from more than one thread. + * */ -class WaypointSequencer +class WaypointSequencer : public mrpt::system::COutputLogger { public: /** ctor */ - WaypointSequencer() = default; + WaypointSequencer() : mrpt::system::COutputLogger("WaypointSequencer") {} /** dtor */ virtual ~WaypointSequencer(); @@ -162,7 +171,7 @@ class WaypointSequencer virtual void resetNavError(); /** Returns the current navigator state. */ - inline NavState getCurrentState() const { return m_navigationState; } + inline NavState getCurrentState() const { return navigationState_; } /** In case of state=NAV_ERROR, this returns the reason for the error. * Error state is reseted every time a new navigation starts with @@ -191,7 +200,7 @@ class WaypointSequencer void endWaypointsAccess() { m_nav_waypoints_cs.unlock(); } /** Publicly available time profiling object. Default: disabled */ - mrpt::system::CTimeLogger m_navProfiler{ + mrpt::system::CTimeLogger navProfiler_{ true /*enabled*/, "WaypointSequencer"}; /** @}*/ @@ -208,10 +217,12 @@ class WaypointSequencer protected: /** Current and last internal state of navigator: */ - NavState m_navigationState = NavState::IDLE; - NavState m_lastNavigationState = NavState::IDLE; + NavState navigationState_ = NavState::IDLE; + NavState lastNavigationState_ = NavState::IDLE; NavErrorReason m_navErrorReason; + bool initialized_ = false; + /** mutex for all navigation methods */ std::recursive_mutex m_nav_cs; @@ -245,7 +256,7 @@ class WaypointSequencer /** Events generated during navigationStep(), enqueued to be called at the * end of the method execution to avoid user code to change the navigator * state. */ - std::list> m_pending_events; + std::list> pendingEvents_; void dispatchPendingNavEvents(); @@ -262,8 +273,7 @@ class WaypointSequencer * `call_virtual_nav_method` can be set to false to avoid calling the * virtual method performNavigationStep() */ - virtual void performNavigationStepNavigating( - bool call_virtual_nav_method = true); + virtual void performNavigationStepNavigating(); /** Will be false until the navigation end is sent, and it is reset with * each new command */ @@ -284,6 +294,8 @@ class WaypointSequencer // virtual bool impl_waypoint_is_reachable( // const mrpt::math::TPoint2D& wp_local_wrt_robot) const = 0; + void internal_on_start_new_navigation(); + #if 0 bool checkHasReachedTarget(const double targetDist) const override; diff --git a/libselfdriving/include/selfdriving/data/Waypoints.h b/libselfdriving/include/selfdriving/data/Waypoints.h index 579efad..d711d44 100644 --- a/libselfdriving/include/selfdriving/data/Waypoints.h +++ b/libselfdriving/include/selfdriving/data/Waypoints.h @@ -196,13 +196,13 @@ struct WaypointStatusSequence mrpt::system::TTimeStamp timestamp_nav_started = INVALID_TIMESTAMP; /** Whether the final waypoint has been reached successfuly. */ - bool final_goal_reached{false}; + bool final_goal_reached = false; /** Index in `waypoints` of the waypoint the navigator is currently trying * to reach. * This will point to the last waypoint after navigation ends successfully. - * Its value is `-1` if navigation has not started yet */ - int waypoint_index_current_goal{-1}; + * It has no value if navigation has not started yet */ + std::optional waypoint_index_current_goal; /** Robot pose at last time step (has INVALID_NUM fields upon * initialization) */ diff --git a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h b/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h index aa694e0..6c848e5 100644 --- a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h +++ b/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h @@ -7,14 +7,21 @@ #pragma once #include -#include -#include -#include +#include +#include +#include +#include namespace selfdriving { using mrpt::kinematics::CVehicleVelCmd; +enum class STOP_TYPE : uint8_t +{ + REGULAR = 0, + EMERGENCY +}; + /** The virtual interface between a path follower / plan executor and a real * mobile platform (vehicle, robot), regarding controlling its motion. * @@ -41,12 +48,15 @@ using mrpt::kinematics::CVehicleVelCmd; * * */ -class VehicleMotionInterface +class VehicleMotionInterface : public mrpt::system::COutputLogger { public: using Ptr = std::shared_ptr; - VehicleMotionInterface() = default; + VehicleMotionInterface() + : mrpt::system::COutputLogger("VehicleMotionInterface") + { + } virtual ~VehicleMotionInterface() = default; /** Provides access to the vehicle localization data. @@ -114,6 +124,36 @@ class VehicleMotionInterface virtual bool motion_execute( const std::optional& immediate, const std::optional& next) = 0; + + /** Stops the vehicle. Different levels of abruptness in the stop can be + * considered given the emergency condition or not of the command. + */ + virtual void stop(const STOP_TYPE stopType) = 0; + + virtual void stop_watchdog() + { + MRPT_LOG_INFO("Default stop_watchdog() called."); + } + virtual void start_watchdog([ + [maybe_unused]] const size_t periodMilliseconds) + { + MRPT_LOG_INFO("Default start_watchdog() called."); + } + + /** @name Event callbacks + * @{ */ + + virtual void on_nav_end_due_to_error() + { + MRPT_LOG_WARN("Default on_nav_end_due_to_error() called."); + } + + virtual void on_nav_start() + { + MRPT_LOG_WARN("Default on_nav_start() event handler called."); + } + + /** @} */ }; } // namespace selfdriving diff --git a/libselfdriving/src/algos/WaypointSequencer.cpp b/libselfdriving/src/algos/WaypointSequencer.cpp index 07fcef4..6ba244d 100644 --- a/libselfdriving/src/algos/WaypointSequencer.cpp +++ b/libselfdriving/src/algos/WaypointSequencer.cpp @@ -5,6 +5,7 @@ * ------------------------------------------------------------------------- */ #include +#include #include using namespace selfdriving; @@ -16,23 +17,173 @@ WaypointSequencer::~WaypointSequencer() void WaypointSequencer::initialize() { + MRPT_START + auto lck = mrpt::lockHelper(m_nav_cs); + // Check that config_ holds all the required fields: + ASSERT_(config_.vehicleMotionInterface); + ASSERT_(config_.obstacleSource); + + // Initialize PTGs... + + initialized_ = true; + + MRPT_END } void WaypointSequencer::requestNavigation(const WaypointSequence& navRequest) { - // + MRPT_START + auto lck = mrpt::lockHelper(m_nav_cs); + ASSERTMSG_(initialized_, "requestNavigation() called before initialize()"); + + m_navigationEndEventSent = false; + + std::lock_guard csl(m_nav_waypoints_cs); + + const size_t N = navRequest.waypoints.size(); + ASSERTMSG_(N > 0, "List of waypoints is empty!"); + + // reset fields to default: + m_waypoint_nav_status = WaypointStatusSequence(); + + m_waypoint_nav_status.waypoints.resize(N); + // Copy waypoints fields data, leave status fields to defaults: + for (size_t i = 0; i < N; i++) + { + ASSERT_(navRequest.waypoints[i].isValid()); + m_waypoint_nav_status.waypoints[i].Waypoint::operator=( + navRequest.waypoints[i]); + } + m_waypoint_nav_status.timestamp_nav_started = mrpt::Clock::now(); + + // new state: + navigationState_ = NavState::NAVIGATING; + m_navErrorReason = NavErrorReason(); + + // Reset the bad navigation alarm: + m_badNavAlarm_minDistTarget = std::numeric_limits::max(); + m_badNavAlarm_lastMinDistTime = mrpt::Clock::now(); + + MRPT_LOG_DEBUG_STREAM( + "requestNavigation() called, navigation plan:\n" + << m_waypoint_nav_status.getAsText()); + + // The main loop navigationStep() will iterate over waypoints + MRPT_END } void WaypointSequencer::navigationStep() { - // + auto lck = mrpt::lockHelper(m_nav_cs); + + ASSERTMSG_(initialized_, "navigationStep() called before initialize()"); + + mrpt::system::CTimeLoggerEntry tle( + navProfiler_, "WaypointSequencer::navigationStep()"); + + const NavState prevState = navigationState_; + switch (navigationState_) + { + case NavState::IDLE: + case NavState::SUSPENDED: + if (lastNavigationState_ == NavState::NAVIGATING) + { + MRPT_LOG_INFO( + "WaypointSequencer::navigationStep(): Navigation stopped."); + } + break; + + case NavState::NAV_ERROR: + // Send end-of-navigation event: + if (lastNavigationState_ == NavState::NAVIGATING && + navigationState_ == NavState::NAV_ERROR) + { + pendingEvents_.emplace_back([this]() { + ASSERT_(config_.vehicleMotionInterface); + config_.vehicleMotionInterface->on_nav_end_due_to_error(); + }); + } + + // If we just arrived at this state, stop the robot: + if (lastNavigationState_ == NavState::NAVIGATING) + { + MRPT_LOG_ERROR( + "[WaypointSequencer::navigationStep()] Stopping navigation " + "due to a NavState::NAV_ERROR state!"); + + if (config_.vehicleMotionInterface) + { + config_.vehicleMotionInterface->stop(STOP_TYPE::REGULAR); + config_.vehicleMotionInterface->stop_watchdog(); + } + } + break; + + case NavState::NAVIGATING: + performNavigationStepNavigating(); + break; + }; + + lastNavigationState_ = prevState; + + dispatchPendingNavEvents(); +} + +void WaypointSequencer::cancel() +{ + auto lck = mrpt::lockHelper(m_nav_cs); + ASSERTMSG_(initialized_, "cancel() called before initialize()"); + + MRPT_LOG_DEBUG("WaypointSequencer::cancel() called."); + navigationState_ = NavState::IDLE; + + if (config_.vehicleMotionInterface) + { + config_.vehicleMotionInterface->stop(STOP_TYPE::REGULAR); + config_.vehicleMotionInterface->stop_watchdog(); + } +} +void WaypointSequencer::resume() +{ + auto lck = mrpt::lockHelper(m_nav_cs); + ASSERTMSG_(initialized_, "resume() called before initialize()"); + + MRPT_LOG_DEBUG("WaypointSequencer::resume() called."); + + if (navigationState_ == NavState::SUSPENDED) + navigationState_ = NavState::NAVIGATING; +} +void WaypointSequencer::suspend() +{ + auto lck = mrpt::lockHelper(m_nav_cs); + ASSERTMSG_(initialized_, "suspend() called before initialize()"); + + MRPT_LOG_DEBUG("WaypointSequencer::suspend() called."); + + if (navigationState_ == NavState::NAVIGATING) + { + navigationState_ = NavState::SUSPENDED; + + if (config_.vehicleMotionInterface) + { + config_.vehicleMotionInterface->stop(STOP_TYPE::REGULAR); + config_.vehicleMotionInterface->stop_watchdog(); + } + } } -void WaypointSequencer::cancel() {} -void WaypointSequencer::resume() {} -void WaypointSequencer::suspend() {} -void WaypointSequencer::resetNavError() {} +void WaypointSequencer::resetNavError() +{ + auto lck = mrpt::lockHelper(m_nav_cs); + ASSERTMSG_(initialized_, "resetNavError() called before initialize()"); + + if (navigationState_ == NavState::NAV_ERROR) + { + navigationState_ = NavState::IDLE; + m_navErrorReason = NavErrorReason(); + } +} WaypointStatusSequence WaypointSequencer::getWaypointNavStatus() const { @@ -43,11 +194,85 @@ WaypointStatusSequence WaypointSequencer::getWaypointNavStatus() const return ret; } -void WaypointSequencer::dispatchPendingNavEvents() {} +void WaypointSequencer::dispatchPendingNavEvents() +{ + // Invoke pending events: + for (auto& ev : pendingEvents_) + { + try + { + ev(); + } + catch (const std::exception& e) + { + MRPT_LOG_ERROR_STREAM("Exception in event handler: " << e.what()); + } + } + pendingEvents_.clear(); +} void WaypointSequencer::updateCurrentPoseAndSpeeds() {} -void WaypointSequencer::performNavigationStepNavigating( - bool call_virtual_nav_method) +void WaypointSequencer::performNavigationStepNavigating() { + const auto prevState = navigationState_; + try + { + if (lastNavigationState_ != NavState::NAVIGATING) + { + MRPT_LOG_INFO( + "[WaypointSequencer::navigationStep()] Starting navigation. " + "Watchdog enabled.\n"); + + internal_on_start_new_navigation(); + } + + // Have we just started the navigation? + if (lastNavigationState_ == NavState::IDLE) + { + pendingEvents_.emplace_back([this]() { + ASSERT_(config_.vehicleMotionInterface); + config_.vehicleMotionInterface->on_nav_start(); + }); + } + + // Get current robot kinematic state: + updateCurrentPoseAndSpeeds(); + + // Have we reached the target location + // TODO... here? + + // Check if the target seems to be at reach, but it's clearly + // occupied by obstacles: + // TODO... here? + // m_counter_check_target_is_blocked = 0; + } + catch (const std::exception& e) + { + navigationState_ = NavState::NAV_ERROR; + if (m_navErrorReason.error_code == NavError::NONE) + { + m_navErrorReason.error_code = NavError::OTHER; + m_navErrorReason.error_msg = + std::string("Exception: ") + std::string(e.what()); + } + + MRPT_LOG_ERROR_FMT( + "[CAbstractNavigator::navigationStep] Exception:\n %s", e.what()); + + // Not useful here? + // if (m_rethrow_exceptions) throw; + } + + navigationState_ = prevState; +} + +void WaypointSequencer::internal_on_start_new_navigation() +{ + ASSERT_(config_.vehicleMotionInterface); + + config_.vehicleMotionInterface->start_watchdog(1000 /*ms*/); + + m_latestPoses.clear(); // Clear cache of last poses. + m_latestOdomPoses.clear(); } diff --git a/libselfdriving/src/data/Waypoints.cpp b/libselfdriving/src/data/Waypoints.cpp index 8ab1589..ce949b0 100644 --- a/libselfdriving/src/data/Waypoints.cpp +++ b/libselfdriving/src/data/Waypoints.cpp @@ -141,8 +141,11 @@ std::string WaypointStatusSequence::getAsText() const s += "\n"; } s += mrpt::format( - " final_goal_reached:%s waypoint_index_current_goal=%d\n", - (final_goal_reached ? "YES" : "NO "), waypoint_index_current_goal); + " final_goal_reached:%s waypoint_index_current_goal=%s\n", + (final_goal_reached ? "YES" : "NO "), + waypoint_index_current_goal + ? std::to_string(*waypoint_index_current_goal).c_str() + : "(Not started)"); return s; }