rnav: More flexible callback behavior

Added:
- new INI file parameter
- callback can now return a bool to reflect whether to cancel navigation
or not

Author: jlblancoc

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();

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);
 
 		/** @} */
 

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;
 				}
 			}
 		}

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() {

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]

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]

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]