diff --git a/libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h b/libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h index ac30b4e2f3..e1ba55fd26 100644 --- a/libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h +++ b/libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h @@ -145,6 +145,7 @@ namespace mrpt double dist_to_target_for_sending_event; //!< Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request. double alarm_seems_not_approaching_target_timeout; //!< navigator timeout (seconds) [Default=30 sec] double dist_check_target_is_blocked; //!< (Default value=0.6) When closer than this distance, check if the target is blocked to abort navigation with an error. + int hysteresis_check_target_is_blocked; // (Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE; virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE; @@ -159,6 +160,7 @@ namespace mrpt private: TState m_lastNavigationState; //!< Last internal state of navigator: bool m_navigationEndEventSent; //!< Will be false until the navigation end is sent, and it is reset with each new command + int m_counter_check_target_is_blocked; /** Called before starting a new navigation. Internally, it calls to child-implemented onStartNewNavigation() */ void internal_onStartNewNavigation(); diff --git a/libs/nav/include/mrpt/nav/reactive/CRobot2NavInterface.h b/libs/nav/include/mrpt/nav/reactive/CRobot2NavInterface.h index 2bcedbef45..1c7570fcbc 100644 --- a/libs/nav/include/mrpt/nav/reactive/CRobot2NavInterface.h +++ b/libs/nav/include/mrpt/nav/reactive/CRobot2NavInterface.h @@ -132,8 +132,11 @@ namespace mrpt /** Callback: Apparent collision event (i.e. there is at least one obstacle point inside the robot shape) */ virtual void sendApparentCollisionEvent(); - /** Callback: Target seems to be blocked by an obstacle. This event is invoked before ending navigation with an ERROR state and another call to sendWaySeemsBlockedEvent(). */ - virtual void sendCannotGetCloserToBlockedTargetEvent(); + /** Callback: Target seems to be blocked by an obstacle. If user + * sets `do_abort_nav` to `true` (default is `false`), after this + * callback returns, navigation will end with an ERROR state and + * another call to sendWaySeemsBlockedEvent() will be done. */ + virtual void sendCannotGetCloserToBlockedTargetEvent(bool &do_abort_nav); /** @} */ diff --git a/libs/nav/src/reactive/CAbstractNavigator.cpp b/libs/nav/src/reactive/CAbstractNavigator.cpp index 9ab128a6b0..3760f460dc 100644 --- a/libs/nav/src/reactive/CAbstractNavigator.cpp +++ b/libs/nav/src/reactive/CAbstractNavigator.cpp @@ -87,6 +87,7 @@ CAbstractNavigator::CAbstractNavigator(CRobot2NavInterface &react_iterf_impl) : mrpt::utils::COutputLogger("MRPT_navigator"), m_lastNavigationState ( IDLE ), m_navigationEndEventSent(false), + m_counter_check_target_is_blocked(0), m_navigationState ( IDLE ), m_navigationParams ( nullptr ), m_robot ( react_iterf_impl ), @@ -354,20 +355,23 @@ bool CAbstractNavigator::stop(bool isEmergencyStop) CAbstractNavigator::TAbstractNavigatorParams::TAbstractNavigatorParams() : dist_to_target_for_sending_event(0), alarm_seems_not_approaching_target_timeout(30), - dist_check_target_is_blocked(0.6) + dist_check_target_is_blocked(0.6), + hysteresis_check_target_is_blocked(3) { } void CAbstractNavigator::TAbstractNavigatorParams::loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) { MRPT_LOAD_CONFIG_VAR_CS(dist_to_target_for_sending_event, double); MRPT_LOAD_CONFIG_VAR_CS(alarm_seems_not_approaching_target_timeout, double); - MRPT_LOAD_CONFIG_VAR_CS(dist_check_target_is_blocked, double); + MRPT_LOAD_CONFIG_VAR_CS(dist_check_target_is_blocked, double); + MRPT_LOAD_CONFIG_VAR_CS(hysteresis_check_target_is_blocked, int); } void CAbstractNavigator::TAbstractNavigatorParams::saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const { MRPT_SAVE_CONFIG_VAR_COMMENT(dist_to_target_for_sending_event, "Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request."); MRPT_SAVE_CONFIG_VAR_COMMENT(alarm_seems_not_approaching_target_timeout, "navigator timeout (seconds) [Default=30 sec]"); MRPT_SAVE_CONFIG_VAR_COMMENT(dist_check_target_is_blocked, "When closer than this distance, check if the target is blocked to abort navigation with an error. [Default=0.6 m]"); + MRPT_SAVE_CONFIG_VAR_COMMENT(hysteresis_check_target_is_blocked, "How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event"); } bool mrpt::nav::operator==(const CAbstractNavigator::TNavigationParamsBase& a, const CAbstractNavigator::TNavigationParamsBase&b) @@ -473,18 +477,36 @@ void CAbstractNavigator::performNavigationStepNavigating(bool call_virtual_nav_m } // Check if the target seems to be at reach, but it's clearly occupied by obstacles: - if ( (!m_navigationParams->target.targetIsIntermediaryWaypoint || m_navigationParams->target.targetDesiredRelSpeed < 1 ) && - targetDist < params_abstract_navigator.dist_check_target_is_blocked) + if (targetDist < params_abstract_navigator.dist_check_target_is_blocked) { const auto rel_trg = m_navigationParams->target.target_coords - m_curPoseVel.pose; const bool is_col = checkCollisionWithLatestObstacles(rel_trg); if (is_col) { - MRPT_LOG_WARN("Target seems to be blocked by obstacles. Aborting navigation."); - m_navigationState = NAV_ERROR; - m_robot.sendCannotGetCloserToBlockedTargetEvent(); - m_robot.sendWaySeemsBlockedEvent(); - return; + const bool send_event = (++m_counter_check_target_is_blocked >= params_abstract_navigator.hysteresis_check_target_is_blocked); + + if (send_event) + { + MRPT_LOG_THROTTLE_WARN(5.0,"Target seems to be blocked by obstacles. Invoking sendCannotGetCloserToBlockedTargetEvent()."); + bool do_abort_nav = false; + m_robot.sendCannotGetCloserToBlockedTargetEvent(do_abort_nav); + + if (do_abort_nav) + { + MRPT_LOG_WARN("sendCannotGetCloserToBlockedTargetEvent() told me to abort navigation."); + m_navigationState = NAV_ERROR; + m_robot.sendWaySeemsBlockedEvent(); + return; + } + else + { + m_counter_check_target_is_blocked = 0; + } + } + } + else + { + m_counter_check_target_is_blocked = 0; } } } diff --git a/libs/nav/src/reactive/CRobot2NavInterface.cpp b/libs/nav/src/reactive/CRobot2NavInterface.cpp index 22d71308d4..ca3a6a22df 100644 --- a/libs/nav/src/reactive/CRobot2NavInterface.cpp +++ b/libs/nav/src/reactive/CRobot2NavInterface.cpp @@ -61,19 +61,19 @@ void CRobot2NavInterface::sendNewWaypointTargetEvent(int waypoint_index) } void CRobot2NavInterface::sendNavigationEndDueToErrorEvent() { - MRPT_LOG_INFO("[sendNavigationEndDueToErrorEvent] Doing nothing: not implemented in user's derived class."); + MRPT_LOG_THROTTLE_INFO(1.0,"[sendNavigationEndDueToErrorEvent] Doing nothing: not implemented in user's derived class."); } void CRobot2NavInterface::sendWaySeemsBlockedEvent() { - MRPT_LOG_INFO("[sendWaySeemsBlockedEvent] Doing nothing: not implemented in user's derived class."); + MRPT_LOG_THROTTLE_INFO(1.0, "[sendWaySeemsBlockedEvent] Doing nothing: not implemented in user's derived class."); } void CRobot2NavInterface::sendApparentCollisionEvent() { - MRPT_LOG_INFO("[sendApparentCollisionEvent] Doing nothing: not implemented in user's derived class."); + MRPT_LOG_THROTTLE_INFO(1.0,"[sendApparentCollisionEvent] Doing nothing: not implemented in user's derived class."); } -void CRobot2NavInterface::sendCannotGetCloserToBlockedTargetEvent() +void CRobot2NavInterface::sendCannotGetCloserToBlockedTargetEvent(bool &do_abort_nav) { - MRPT_LOG_INFO("[sendCannotGetCloserToBlockedTargetEvent] Doing nothing: not implemented in user's derived class."); + MRPT_LOG_THROTTLE_INFO(1.0, "[sendCannotGetCloserToBlockedTargetEvent] Doing nothing: not implemented in user's derived class."); } double CRobot2NavInterface::getNavigationTime() { diff --git a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini index 729e1a943d..aab42a6d5f 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini @@ -14,7 +14,8 @@ [CAbstractNavigator] dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request. alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec] -dist_check_target_is_blocked = 1.0 // When closer than this distance, check if the target is blocked to abort navigation with an error +dist_check_target_is_blocked = 2.0 // When closer than this distance, check if the target is blocked to abort navigation with an error +hysteresis_check_target_is_blocked = 3 // How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event [CWaypointsNavigator] diff --git a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini index a2d0616dc0..97bde861ba 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini @@ -15,7 +15,8 @@ [CAbstractNavigator] dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request. alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec] -dist_check_target_is_blocked = 1.0 // When closer than this distance, check if the target is blocked to abort navigation with an error +dist_check_target_is_blocked = 2.0 // When closer than this distance, check if the target is blocked to abort navigation with an error +hysteresis_check_target_is_blocked = 3 // How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event [CWaypointsNavigator] diff --git a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini index 77768f0cf0..e653013b13 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini @@ -21,6 +21,7 @@ dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request. alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec] dist_check_target_is_blocked = 2.0 // When closer than this distance, check if the target is blocked to abort navigation with an error +hysteresis_check_target_is_blocked = 3 // How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event [DIFF_CWaypointsNavigator]