Skip to content

Commit

Permalink
rnav: More flexible callback behavior
Browse files Browse the repository at this point in the history 
Added:
- new INI file parameter
- callback can now return a bool to reflect whether to cancel navigation
or not
  • Loading branch information
@jlblancoc
jlblancoc committed Jun 21, 2017
1 parent 3f7e594 commit 752b211
Show file tree
Hide file tree
Showing 7 changed files with 48 additions and 18 deletions.
2 changes: 2 additions & 0 deletions libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();
Expand Down
7 changes: 5 additions & 2 deletions libs/nav/include/mrpt/nav/reactive/CRobot2NavInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

/** @} */

Expand Down
40 changes: 31 additions & 9 deletions libs/nav/src/reactive/CAbstractNavigator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ),
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
}
}
}
Expand Down
10 changes: 5 additions & 5 deletions libs/nav/src/reactive/CRobot2NavInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
3 changes: 2 additions & 1 deletion share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
3 changes: 2 additions & 1 deletion share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
1 change: 1 addition & 0 deletions share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down

0 comments on commit 752b211

Please sign in to comment.