Permalink
Browse files

rnav: More flexible callback behavior

Added:
- new INI file parameter
- callback can now return a bool to reflect whether to cancel navigation
or not
  • Loading branch information...
jlblancoc committed Jun 21, 2017
1 parent 3f7e594 commit 752b211002016751e6108ab3e90908d7572b6fe0
@@ -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();
@@ -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);
/** @} */
@@ -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;
}
}
}
@@ -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() {
@@ -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]
@@ -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]
@@ -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]

0 comments on commit 752b211

Please sign in to comment.