Пример #1
0
/*---------------------------------------------------------------
					navigationStep
  ---------------------------------------------------------------*/
void CAbstractNavigator::navigationStep()
{
	mrpt::synch::CCriticalSectionLocker csl(&m_nav_cs);
	mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays, "CAbstractNavigator::navigationStep()");

	const TState prevState = m_navigationState;
	switch ( m_navigationState )
	{
	case IDLE:
	case SUSPENDED:
		try
		{
			// If we just arrived at this state, stop robot:
			if ( m_lastNavigationState == NAVIGATING )
			{
				MRPT_LOG_INFO("[CAbstractNavigator::navigationStep()] Navigation stopped.");
				// this->stop();  stop() is called by the method switching the "state", so we have more flexibility
				m_robot.stopWatchdog();
			}
		} catch (...) { }
		break;

	case NAV_ERROR:
		try
		{
			// Send end-of-navigation event:
			if ( m_lastNavigationState == NAVIGATING && m_navigationState == NAV_ERROR)
				m_robot.sendNavigationEndDueToErrorEvent();

			// If we just arrived at this state, stop the robot:
			if ( m_lastNavigationState == NAVIGATING )
			{
				MRPT_LOG_ERROR("[CAbstractNavigator::navigationStep()] Stoping Navigation due to a NAV_ERROR state!");
				this->stop(false /*not emergency*/);
				m_robot.stopWatchdog();
			}
		} catch (...) { }
		break;

	case NAVIGATING:
		try
		{
			if ( m_lastNavigationState != NAVIGATING )
			{
				MRPT_LOG_INFO("[CAbstractNavigator::navigationStep()] Starting Navigation. Watchdog initiated...\n");
				if (m_navigationParams)
					MRPT_LOG_DEBUG(mrpt::format("[CAbstractNavigator::navigationStep()] Navigation Params:\n%s\n", m_navigationParams->getAsText().c_str() ));

				m_robot.startWatchdog( 1000 );	// Watchdog = 1 seg
				m_latestPoses.clear(); // Clear cache of last poses.
				onStartNewNavigation();
			}

			// Have we just started the navigation?
			if ( m_lastNavigationState == IDLE )
				m_robot.sendNavigationStartEvent();

			/* ----------------------------------------------------------------
			        Get current robot dyn state:
				---------------------------------------------------------------- */
			updateCurrentPoseAndSpeeds();

			/* ----------------------------------------------------------------
		 			Have we reached the target location?
				---------------------------------------------------------------- */
			// Build a 2D segment from the current robot pose to the previous one:
			ASSERT_(!m_latestPoses.empty());
			const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
				mrpt::math::TPoint2D(m_curPoseVel.pose),
				m_latestPoses.size()>1 ?
					mrpt::math::TPoint2D( (++m_latestPoses.rbegin())->second )
					:
					mrpt::math::TPoint2D((m_latestPoses.rbegin())->second)
			);

			const double targetDist = seg_robot_mov.distance( mrpt::math::TPoint2D(m_navigationParams->target) );

			// Should "End of navigation" event be sent??
			if (!m_navigationParams->targetIsIntermediaryWaypoint && !m_navigationEndEventSent && targetDist < params_abstract_navigator.dist_to_target_for_sending_event)
			{
				m_navigationEndEventSent = true;
				m_robot.sendNavigationEndEvent();
			}

			// Have we really reached the target?
			if ( targetDist < m_navigationParams->targetAllowedDistance )
			{
				m_lastNavTargetReached = true;

				if (!m_navigationParams->targetIsIntermediaryWaypoint) {
					this->stop(false /*not emergency*/);
				}
				m_navigationState = IDLE;
				logFmt(mrpt::utils::LVL_WARN, "Navigation target (%.03f,%.03f) was reached\n", m_navigationParams->target.x,m_navigationParams->target.y);

				if (!m_navigationParams->targetIsIntermediaryWaypoint && !m_navigationEndEventSent)
				{
					m_navigationEndEventSent = true;
					m_robot.sendNavigationEndEvent();
				}
				break;
			}

			// Check the "no approaching the target"-alarm:
			// -----------------------------------------------------------
			if (targetDist < m_badNavAlarm_minDistTarget )
			{
				m_badNavAlarm_minDistTarget = targetDist;
				m_badNavAlarm_lastMinDistTime =  mrpt::system::getCurrentTime();
			}
			else
			{
				// Too much time have passed?
				if (mrpt::system::timeDifference( m_badNavAlarm_lastMinDistTime, mrpt::system::getCurrentTime() ) > params_abstract_navigator.alarm_seems_not_approaching_target_timeout)
				{
					MRPT_LOG_WARN("--------------------------------------------\nWARNING: Timeout for approaching toward the target expired!! Aborting navigation!! \n---------------------------------\n");
					m_navigationState = NAV_ERROR;
					m_robot.sendWaySeemsBlockedEvent();
					break;
				}
			}

			// ==== The normal execution of the navigation: Execute one step ====
			performNavigationStep();

		}
		catch (std::exception &e)
		{
			cerr << "[CAbstractNavigator::navigationStep] Exception:\n" << e.what() << endl;
		}
		catch (...)
		{
			cerr << "[CAbstractNavigator::navigationStep] Unexpected exception.\n";
		}
		break;	// End case NAVIGATING
	};
	m_lastNavigationState = prevState;
}
Пример #2
0
/** \callergraph */
void CAbstractNavigator::performNavigationStepNavigating(
	bool call_virtual_nav_method)
{
	const TState prevState = m_navigationState;
	try
	{
		if (m_lastNavigationState != NAVIGATING)
		{
			MRPT_LOG_INFO(
				"[CAbstractNavigator::navigationStep()] Starting Navigation. "
				"Watchdog initiated...\n");
			if (m_navigationParams)
				MRPT_LOG_DEBUG(mrpt::format(
					"[CAbstractNavigator::navigationStep()] Navigation "
					"Params:\n%s\n",
					m_navigationParams->getAsText().c_str()));

			internal_onStartNewNavigation();
		}

		// Have we just started the navigation?
		if (m_lastNavigationState == IDLE)
		{
			m_pending_events.push_back(std::bind(
				&CRobot2NavInterface::sendNavigationStartEvent,
				std::ref(m_robot)));
		}

		/* ----------------------------------------------------------------
		Get current robot dyn state:
		---------------------------------------------------------------- */
		updateCurrentPoseAndSpeeds();

		/* ----------------------------------------------------------------
		Have we reached the target location?
		---------------------------------------------------------------- */
		// Build a 2D segment from the current robot pose to the previous one:
		ASSERT_(!m_latestPoses.empty());
		const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
			mrpt::math::TPoint2D(m_curPoseVel.pose),
			m_latestPoses.size() > 1
				? mrpt::math::TPoint2D((++m_latestPoses.rbegin())->second)
				: mrpt::math::TPoint2D((m_latestPoses.rbegin())->second));

		if (m_navigationParams)
		{
			const double targetDist = seg_robot_mov.distance(
				mrpt::math::TPoint2D(m_navigationParams->target.target_coords));

			// Should "End of navigation" event be sent??
			if (!m_navigationParams->target.targetIsIntermediaryWaypoint &&
				!m_navigationEndEventSent &&
				targetDist <
					params_abstract_navigator.dist_to_target_for_sending_event)
			{
				m_navigationEndEventSent = true;
				m_pending_events.push_back(std::bind(
					&CRobot2NavInterface::sendNavigationEndEvent,
					std::ref(m_robot)));
			}

			// Have we really reached the target?
			if (checkHasReachedTarget(targetDist))
			{
				m_navigationState = IDLE;
				logFmt(
					mrpt::system::LVL_WARN,
					"Navigation target (%.03f,%.03f) was reached\n",
					m_navigationParams->target.target_coords.x,
					m_navigationParams->target.target_coords.y);

				if (!m_navigationParams->target.targetIsIntermediaryWaypoint)
				{
					this->stop(false /*not emergency*/);
					if (!m_navigationEndEventSent)
					{
						m_navigationEndEventSent = true;
						m_pending_events.push_back(std::bind(
							&CRobot2NavInterface::sendNavigationEndEvent,
							std::ref(m_robot)));
					}
				}
				return;
			}

			// Check the "no approaching the target"-alarm:
			// -----------------------------------------------------------
			if (targetDist < m_badNavAlarm_minDistTarget)
			{
				m_badNavAlarm_minDistTarget = targetDist;
				m_badNavAlarm_lastMinDistTime = mrpt::system::getCurrentTime();
			}
			else
			{
				// Too much time have passed?
				if (mrpt::system::timeDifference(
						m_badNavAlarm_lastMinDistTime,
						mrpt::system::getCurrentTime()) >
					params_abstract_navigator
						.alarm_seems_not_approaching_target_timeout)
				{
					MRPT_LOG_WARN(
						"Timeout approaching the target. Aborting navigation.");

					m_navigationState = NAV_ERROR;

					m_pending_events.push_back(std::bind(
						&CRobot2NavInterface::sendWaySeemsBlockedEvent,
						std::ref(m_robot)));
					return;
				}
			}

			// Check if the target seems to be at reach, but it's clearly
			// occupied by obstacles:
			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)
				{
					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().");

						m_pending_events.push_back(std::bind(
							&CRobot2NavInterface::
								sendCannotGetCloserToBlockedTargetEvent,
							std::ref(m_robot)));

						m_counter_check_target_is_blocked = 0;
					}
				}
				else
				{
					m_counter_check_target_is_blocked = 0;
				}
			}
		}

		// The normal execution of the navigation: Execute one step:
		if (call_virtual_nav_method)
		{
			performNavigationStep();
		}
	}
	catch (std::exception& e)
	{
		MRPT_LOG_ERROR_FMT(
			"[CAbstractNavigator::navigationStep] Exception:\n %s", e.what());
	}
	catch (...)
	{
		MRPT_LOG_ERROR(
			"[CAbstractNavigator::navigationStep] Untyped exception!");
	}
	m_navigationState = prevState;
}