Class defining the state transition table behavior for the LocalizationManager. More...

#include <LocalizationTransitionTable.hpp>

Collaboration diagram for localization_manager::LocalizationTransitionTable:

Public Types
using	TransitionCallback = std::function< void(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal)>

Public Member Functions
	LocalizationTransitionTable (LocalizerMode mode)
	Constructor. More...

LocalizationState	getState () const
	Returns the current state. More...

void	signal (LocalizationSignal signal)
	Trigger signal for the transition table. More...

void	setTransitionCallback (TransitionCallback cb)
	Callback setting function. The provided callback will be triggered any time the current state changes to a new state. More...

Private Member Functions
void	signalWhenUNINITIALIZED (LocalizationSignal signal)

void	signalWhenINITIALIZING (LocalizationSignal signal)

void	signalWhenOPERATIONAL (LocalizationSignal signal)

void	signalWhenDEGRADED (LocalizationSignal signal)

void	signalWhenDEGRADED_NO_LIDAR_FIX (LocalizationSignal signal)

void	signalWhenAWAIT_MANUAL_INITIALIZATION (LocalizationSignal signal)

void	logDebugSignal (LocalizationSignal signal) const
	Helper function for logging the provide signal. More...

void	setAndLogState (LocalizationState new_state, LocalizationSignal source_signal)
	Function to change the current state and log the details of the transition. More...

Private Attributes
LocalizationState	state_ = LocalizationState::UNINITIALIZED
	Current state. This state should only ever be set using the setAndLogState() function. More...

LocalizerMode	mode_ = LocalizerMode::AUTO_WITHOUT_TIMEOUT

TransitionCallback	transition_callback_

Detailed Description

Class defining the state transition table behavior for the LocalizationManager.

Definition at line 26 of file LocalizationTransitionTable.hpp.

Member Typedef Documentation

◆ TransitionCallback

using localization_manager::LocalizationTransitionTable::TransitionCallback = std::function<void(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal)>

Definition at line 29 of file LocalizationTransitionTable.hpp.

Constructor & Destructor Documentation

◆ LocalizationTransitionTable()

localization_manager::LocalizationTransitionTable::LocalizationTransitionTable ( LocalizerMode mode )

Constructor.

Parameters

mode	Defines the operational mode of the state machine which modifies some of the state transitions

Definition at line 22 of file LocalizationTransitionTable.cpp.

22: mode_(mode) {}

localization_manager::LocalizationTransitionTable::mode_

LocalizerMode mode_

Definition: LocalizationTransitionTable.hpp:65

Member Function Documentation

◆ getState()

LocalizationState localization_manager::LocalizationTransitionTable::getState ( ) const

Returns the current state.

Returns: Current state

Definition at line 24 of file LocalizationTransitionTable.cpp.

    {
        return state_;
    }

References state_.

Referenced by localization_manager::LocalizationManager::getState(), localization_manager::LocalizationManager::gnssPoseCallback(), localization_manager::LocalizationManager::poseAndStatsCallback(), localization_manager::LocalizationManager::posePubTick(), and localization_manager::LocalizationManager::timerCallback().

Here is the caller graph for this function:

◆ logDebugSignal()

void localization_manager::LocalizationTransitionTable::logDebugSignal ( LocalizationSignal signal ) const

private

Helper function for logging the provide signal.

Parameters

signal The signal to be logged

Definition at line 29 of file LocalizationTransitionTable.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationTransitionTable received unsupported signal of " << signal << "while in state" << state_);
    }

References signal(), and state_.

Referenced by signalWhenAWAIT_MANUAL_INITIALIZATION(), signalWhenDEGRADED(), signalWhenDEGRADED_NO_LIDAR_FIX(), signalWhenINITIALIZING(), signalWhenOPERATIONAL(), and signalWhenUNINITIALIZED().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setAndLogState()

void localization_manager::LocalizationTransitionTable::setAndLogState	(	LocalizationState	new_state,
		LocalizationSignal	source_signal
	)

private

Function to change the current state and log the details of the transition.

Parameters

new_state	The state to set.
source_signal	The signal which caused the new_state to be set

Definition at line 34 of file LocalizationTransitionTable.cpp.

    {
        if (new_state == state_)
        {
            return; // State was unchanged no need to log or trigger callbacks
        }
        RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationTransitionTable changed localization state from " << state_ << " to " << new_state << " because of signal " << source_signal << " while in mode " << mode_);
 
        LocalizationState prev_state = state_;
        state_ = new_state;
        if (transition_callback_)
        {
            transition_callback_(prev_state, state_, source_signal);
        }
    }

References mode_, state_, and transition_callback_.

Referenced by signalWhenAWAIT_MANUAL_INITIALIZATION(), signalWhenDEGRADED(), signalWhenDEGRADED_NO_LIDAR_FIX(), signalWhenINITIALIZING(), signalWhenOPERATIONAL(), and signalWhenUNINITIALIZED().

Here is the caller graph for this function:

◆ setTransitionCallback()

void localization_manager::LocalizationTransitionTable::setTransitionCallback ( TransitionCallback cb )

Callback setting function. The provided callback will be triggered any time the current state changes to a new state.

Parameters

cb	The callback function which will be provided with the previous state, new current state, and the signal which caused the transition.

Definition at line 247 of file LocalizationTransitionTable.cpp.

    {
        transition_callback_ = cb;
    }

References transition_callback_.

Referenced by localization_manager::LocalizationManager::LocalizationManager().

Here is the caller graph for this function:

◆ signal()

void localization_manager::LocalizationTransitionTable::signal ( LocalizationSignal signal )

Trigger signal for the transition table.

Parameters

signal The signal for the transition table to evaluate

Definition at line 219 of file LocalizationTransitionTable.cpp.

    {
        switch (state_)
        {
        case LocalizationState::UNINITIALIZED:
            signalWhenUNINITIALIZED(signal);
            break;
        case LocalizationState::INITIALIZING:
            signalWhenINITIALIZING(signal);
            break;
        case LocalizationState::OPERATIONAL:
            signalWhenOPERATIONAL(signal);
            break;
        case LocalizationState::DEGRADED:
            signalWhenDEGRADED(signal);
            break;
        case LocalizationState::DEGRADED_NO_LIDAR_FIX:
            signalWhenDEGRADED_NO_LIDAR_FIX(signal);
            break;
        case LocalizationState::AWAIT_MANUAL_INITIALIZATION:
            signalWhenAWAIT_MANUAL_INITIALIZATION(signal);
            break;
        default:
            throw std::invalid_argument("Invalid signal passed to LocalizationTransitionTable::signal");
            break;
        }
    }

References localization_manager::AWAIT_MANUAL_INITIALIZATION, localization_manager::DEGRADED, localization_manager::DEGRADED_NO_LIDAR_FIX, localization_manager::INITIALIZING, localization_manager::OPERATIONAL, signal(), signalWhenAWAIT_MANUAL_INITIALIZATION(), signalWhenDEGRADED(), signalWhenDEGRADED_NO_LIDAR_FIX(), signalWhenINITIALIZING(), signalWhenOPERATIONAL(), signalWhenUNINITIALIZED(), state_, and localization_manager::UNINITIALIZED.

Referenced by localization_manager::LocalizationManager::gnssPoseCallback(), localization_manager::LocalizationManager::initialPoseCallback(), logDebugSignal(), localization_manager::LocalizationManager::poseAndStatsCallback(), localization_manager::LocalizationManager::posePubTick(), signal(), signalWhenAWAIT_MANUAL_INITIALIZATION(), signalWhenDEGRADED(), signalWhenDEGRADED_NO_LIDAR_FIX(), signalWhenINITIALIZING(), signalWhenOPERATIONAL(), signalWhenUNINITIALIZED(), localization_manager::LocalizationManager::systemAlertCallback(), and localization_manager::LocalizationManager::timerCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenAWAIT_MANUAL_INITIALIZATION()

void localization_manager::LocalizationTransitionTable::signalWhenAWAIT_MANUAL_INITIALIZATION ( LocalizationSignal signal )

private

Definition at line 199 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        case LocalizationSignal::INITIAL_POSE:
            if (mode_ == LocalizerMode::GNSS || mode_ == LocalizerMode::GNSS_WITH_FIXED_OFFSET)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            else
            {
                setAndLogState(LocalizationState::INITIALIZING, signal);
            }
            break;
        default:
            logDebugSignal(signal);
            break;
        }
    }

References localization_manager::DEGRADED_NO_LIDAR_FIX, localization_manager::GNSS, localization_manager::GNSS_WITH_FIXED_OFFSET, localization_manager::INITIAL_POSE, localization_manager::INITIALIZING, logDebugSignal(), mode_, setAndLogState(), and signal().

Referenced by signal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenDEGRADED()

void localization_manager::LocalizationTransitionTable::signalWhenDEGRADED ( LocalizationSignal signal )

private

Definition at line 133 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        case LocalizationSignal::INITIAL_POSE:
            setAndLogState(LocalizationState::INITIALIZING, signal);
            break;
        case LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE:
            setAndLogState(LocalizationState::OPERATIONAL, signal);
            break;
        case LocalizationSignal::LIDAR_SENSOR_FAILURE:
            if (mode_ == LocalizerMode::AUTO_WITH_TIMEOUT || mode_ == LocalizerMode::AUTO_WITHOUT_TIMEOUT)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            else
            {
                setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            }
            break;
        case LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE:
            if (mode_ == LocalizerMode::AUTO_WITH_TIMEOUT || mode_ == LocalizerMode::AUTO_WITHOUT_TIMEOUT)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            else
            {
                setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            }
            break;
        default:
            logDebugSignal(signal);
            break;
        }
    }

Referenced by signal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenDEGRADED_NO_LIDAR_FIX()

void localization_manager::LocalizationTransitionTable::signalWhenDEGRADED_NO_LIDAR_FIX ( LocalizationSignal signal )

private

Definition at line 169 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        case LocalizationSignal::INITIAL_POSE:
            if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET)
            {
                setAndLogState(LocalizationState::INITIALIZING, signal);
            }
            break;
        case LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE:
            if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET)
            {
                setAndLogState(LocalizationState::OPERATIONAL, signal);
            }
            break;
        case LocalizationSignal::TIMEOUT:
            if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET && mode_ != LocalizerMode::AUTO_WITHOUT_TIMEOUT && mode_ != LocalizerMode::GNSS_WITH_NDT_INIT)
            {
                setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            }
            break;
        case LocalizationSignal::GNSS_DATA_TIMEOUT:
            throw std::runtime_error("GNSS_DATA_TIMEOUT occurred while in DEGRADED_NO_LIDAR_FIX state. Localization cannot recover");
        default:
            logDebugSignal(signal);
            break;
        }
    }

Referenced by signal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenINITIALIZING()

void localization_manager::LocalizationTransitionTable::signalWhenINITIALIZING ( LocalizationSignal signal )

private

Definition at line 70 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        // How to handle the combined conditions?
        case LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE:
            setAndLogState(LocalizationState::OPERATIONAL, signal);
            break;
        case LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE:
            setAndLogState(LocalizationState::DEGRADED, signal);
            break;
        case LocalizationSignal::LIDAR_SENSOR_FAILURE:
            if (mode_ == LocalizerMode::NDT)
            {
                setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            }
            else
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            break;
        case LocalizationSignal::TIMEOUT:
            setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            break;
        default:
            logDebugSignal(signal);
            break;
        }
    }

References localization_manager::AWAIT_MANUAL_INITIALIZATION, localization_manager::DEGRADED, localization_manager::DEGRADED_NO_LIDAR_FIX, localization_manager::GOOD_NDT_FREQ_AND_FITNESS_SCORE, localization_manager::LIDAR_SENSOR_FAILURE, logDebugSignal(), mode_, localization_manager::NDT, localization_manager::OPERATIONAL, localization_manager::POOR_NDT_FREQ_OR_FITNESS_SCORE, setAndLogState(), signal(), and localization_manager::TIMEOUT.

Referenced by signal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenOPERATIONAL()

void localization_manager::LocalizationTransitionTable::signalWhenOPERATIONAL ( LocalizationSignal signal )

private

Definition at line 100 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        case LocalizationSignal::INITIAL_POSE:
            setAndLogState(LocalizationState::INITIALIZING, signal);
            break;
        case LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE:
            setAndLogState(LocalizationState::DEGRADED, signal);
            break;
        case LocalizationSignal::LIDAR_SENSOR_FAILURE: // Allowing fallthrough for duplicated behavior
        case LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE:
            if (mode_ == LocalizerMode::AUTO_WITH_TIMEOUT || mode_ == LocalizerMode::AUTO_WITHOUT_TIMEOUT)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            else
            {
                setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal);
            }
            break;
        case LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS:
            if (mode_ == LocalizerMode::GNSS_WITH_NDT_INIT)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            break;
        default:
            logDebugSignal(signal);
            break;
        }
    }

Referenced by signal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ signalWhenUNINITIALIZED()

void localization_manager::LocalizationTransitionTable::signalWhenUNINITIALIZED ( LocalizationSignal signal )

private

Definition at line 50 of file LocalizationTransitionTable.cpp.

    {
        switch (signal)
        {
        case LocalizationSignal::INITIAL_POSE:
            if (mode_ == LocalizerMode::GNSS || mode_ == LocalizerMode::GNSS_WITH_FIXED_OFFSET)
            {
                setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal);
            }
            else
            {
                setAndLogState(LocalizationState::INITIALIZING, signal);
            }
            break;
        default:
            logDebugSignal(signal);
            break;
        }
    }