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