Primary logic class for the localization manager node. More...

#include <LocalizationManager.hpp>

Collaboration diagram for localization_manager::LocalizationManager:

Public Types
using	PosePublisher = std::function< void(const geometry_msgs::msg::PoseStamped &)>

using	ManagedInitialPosePublisher = std::function< void(const geometry_msgs::msg::PoseWithCovarianceStamped &)>

using	StatePublisher = std::function< void(const carma_localization_msgs::msg::LocalizationStatusReport &)>

using	TimerUniquePtr = std::unique_ptr< carma_ros2_utils::timers::Timer >

Public Member Functions
	LocalizationManager (PosePublisher pose_pub, StatePublisher state_pub, ManagedInitialPosePublisher initialpose_pub, const LocalizationManagerConfig &config, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, std::unique_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory)
	Constructor. More...

void	gnssPoseCallback (const geometry_msgs::msg::PoseStamped::SharedPtr msg)
	Callback for new GNSS messages. More...

void	poseAndStatsCallback (const geometry_msgs::msg::PoseStamped::ConstPtr pose, const autoware_msgs::msg::NDTStat::ConstPtr stats)
	Synced callback for the NDT reported pose and status messages. More...

void	systemAlertCallback (const carma_msgs::msg::SystemAlert::SharedPtr alert)
	Callback for SystemAlert data. Used to check for lidar failure. More...

void	initialPoseCallback (const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
	Callback for the initial pose provided by Rviz or some external localization initializer. More...

void	posePubTick ()
	Timer callback that controls the publication of the selected pose and status report. More...

void	stateTransitionCallback (LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal)
	Callback for when a new state was triggered. Allows creation of entry/exit behavior for states. More...

void	timerCallback (const LocalizationState origin_state)
	Callback for timeouts. Used to trigger timeout signals for the state machine. More...

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

void	clearTimers ()
	Clears the expired timers from the memory of this scheduler. More...

void	setConfig (const LocalizationManagerConfig &config)
	Set config. More...

Private Member Functions
double	computeNDTFreq (const rclcpp::Time &new_stamp)
	Helper function to both compute the NDT Frequency and update the previous pose timestamp. More...

double	computeFreq (const rclcpp::Time &old_stamp, const rclcpp::Time &new_stamp) const
	Helper function to compute the instantaneous frequency between two times. More...

uint32_t	nextId ()
	Generates the next id to be used for a timer. More...

Private Attributes
PosePublisher	pose_pub_

StatePublisher	state_pub_

ManagedInitialPosePublisher	initialpose_pub_

LocalizationManagerConfig	config_

LocalizationTransitionTable	transition_table_

boost::optional< rclcpp::Time >	prev_ndt_stamp_

int	lidar_init_sequential_timesteps_counter_ = 0

bool	is_sequential_ = false

boost::optional< geometry_msgs::msg::PoseStamped >	last_raw_gnss_value_

boost::optional< tf2::Vector3 >	gnss_offset_

boost::optional< geometry_msgs::msg::PoseStamped >	current_pose_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger_

std::mutex	mutex_

std::unique_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory_

std::unordered_map< uint32_t, std::pair< TimerUniquePtr, bool > >	timers_

TimerUniquePtr	current_timer_

uint32_t	next_id_ = 0

uint32_t	current_timer_id_

rcl_clock_type_t	timer_clock_type_ = RCL_SYSTEM_TIME

Static Private Attributes
static const std::unordered_set< std::string >	LIDAR_FAILURE_STRINGS
	The set of strings which mark a lidar failure in a system alert message. More...

Detailed Description

Primary logic class for the localization manager node.

Definition at line 40 of file LocalizationManager.hpp.

Member Typedef Documentation

◆ ManagedInitialPosePublisher

using localization_manager::LocalizationManager::ManagedInitialPosePublisher = std::function<void(const geometry_msgs::msg::PoseWithCovarianceStamped &)>

Definition at line 45 of file LocalizationManager.hpp.

◆ PosePublisher

using localization_manager::LocalizationManager::PosePublisher = std::function<void(const geometry_msgs::msg::PoseStamped &)>

Definition at line 44 of file LocalizationManager.hpp.

◆ StatePublisher

using localization_manager::LocalizationManager::StatePublisher = std::function<void(const carma_localization_msgs::msg::LocalizationStatusReport &)>

Definition at line 46 of file LocalizationManager.hpp.

◆ TimerUniquePtr

using localization_manager::LocalizationManager::TimerUniquePtr = std::unique_ptr<carma_ros2_utils::timers::Timer>

Definition at line 47 of file LocalizationManager.hpp.

Constructor & Destructor Documentation

◆ LocalizationManager()

localization_manager::LocalizationManager::LocalizationManager	(	PosePublisher	pose_pub,
		StatePublisher	state_pub,
		ManagedInitialPosePublisher	initialpose_pub,
		const LocalizationManagerConfig &	config,
		rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger,
		std::unique_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory
	)

Constructor.

Parameters

pose_pub	A callback to trigger publication of the selected pose
state_pub	A callback to trigger publication of the localization state
initialpose_pub	A callback to trigger publication of the intial pose
config	The configuration settings to use for this manager
logger	Logger interface of node calling manager object
node_timers	Timer interface of node calling manager object

Definition at line 26 of file LocalizationManager.cpp.

        : pose_pub_(pose_pub), state_pub_(state_pub), initialpose_pub_(initialpose_pub), config_(config), logger_(logger), timer_factory_(std::move(timer_factory)), transition_table_(static_cast<LocalizerMode>(config_.localization_mode))
 
    {
 
        transition_table_.setTransitionCallback(std::bind(&LocalizationManager::stateTransitionCallback, this,
                                                          std::placeholders::_1, std::placeholders::_2,
                                                          std::placeholders::_3));
        timer_clock_type_ = timer_factory_->now().get_clock_type();
    }

References localization_manager::LocalizationTransitionTable::setTransitionCallback(), stateTransitionCallback(), timer_clock_type_, timer_factory_, and transition_table_.

Here is the call graph for this function:

Member Function Documentation

◆ clearTimers()

void localization_manager::LocalizationManager::clearTimers ( )

Clears the expired timers from the memory of this scheduler.

Definition at line 206 of file LocalizationManager.cpp.

    {
        std::lock_guard<std::mutex> guard(mutex_);
 
        // Clear expired timers
        auto it = timers_.begin();
        while (it != timers_.end())
        {
            // Check if timer is marked for deletion
            if (it->second.second)
            {
                // erase() function returns the iterator of the next to last deleted element
                it = timers_.erase(it);
            }
            else
            {
                it++;
            }
        }
    }

References mutex_, and timers_.

Referenced by posePubTick().

Here is the caller graph for this function:

◆ computeFreq()

double localization_manager::LocalizationManager::computeFreq	(	const rclcpp::Time &	old_stamp,
		const rclcpp::Time &	new_stamp
	)		const

private

Helper function to compute the instantaneous frequency between two times.

Parameters

old_stamp	The old timestamp
new_stamp	The new timestamp

Returns: The computed instantaneous frequency in Hz

Definition at line 41 of file LocalizationManager.cpp.

    {
        return 1.0 / (new_stamp - old_stamp).seconds(); // Convert delta to frequency (Hz = 1/s)
    }

Referenced by computeNDTFreq(), and posePubTick().

Here is the caller graph for this function:

◆ computeNDTFreq()

double localization_manager::LocalizationManager::computeNDTFreq ( const rclcpp::Time & new_stamp )

private

Helper function to both compute the NDT Frequency and update the previous pose timestamp.

Parameters

new_stamp The new pose timestamp

Returns: The computed instantaneous frequency in Hz

Definition at line 51 of file LocalizationManager.cpp.

    {
        if (!prev_ndt_stamp_)
        { // Check if this is the first data point
            prev_ndt_stamp_ = new_stamp;
            // When no historic data is available force the frequency into the operational range
            return config_.ndt_frequency_degraded_threshold * 2;
        }
 
        if (new_stamp <= rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_))
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationManager received NDT data out of order. Prev stamp was "
                                                                                << prev_ndt_stamp_.get().seconds() << " new stamp is " << new_stamp.seconds());
            // When invalid data is received from NDT force the frequency into the fault range
            return config_.ndt_frequency_fault_threshold / 2;
        }
        return computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), new_stamp); // Convert delta to frequency (Hz = 1/s)
    }

References computeFreq(), config_, localization_manager::LocalizationManagerConfig::ndt_frequency_degraded_threshold, localization_manager::LocalizationManagerConfig::ndt_frequency_fault_threshold, prev_ndt_stamp_, and timer_clock_type_.

Referenced by poseAndStatsCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getState()

LocalizationState localization_manager::LocalizationManager::getState ( ) const

Returns the current localization state.

Returns: The current localization state

Definition at line 316 of file LocalizationManager.cpp.

    {
        return transition_table_.getState();
    }

References localization_manager::LocalizationTransitionTable::getState(), and transition_table_.

Here is the call graph for this function:

◆ gnssPoseCallback()

void localization_manager::LocalizationManager::gnssPoseCallback ( const geometry_msgs::msg::PoseStamped::SharedPtr msg )

Callback for new GNSS messages.

Parameters

msg	The pose of vehicle in map frame provided by GNSS

Definition at line 143 of file LocalizationManager.cpp.

    {
        last_raw_gnss_value_ = *msg;
        // Just like ndt_matching the gnss pose is treated as an initialize signal if the system is not yet intialized
        if (transition_table_.getState() == LocalizationState::UNINITIALIZED)
        {
            lidar_init_sequential_timesteps_counter_ = 0;
            transition_table_.signal(LocalizationSignal::INITIAL_POSE);
 
            geometry_msgs::msg::PoseWithCovarianceStamped new_initial_pose;
            new_initial_pose.header = msg->header;
            new_initial_pose.pose.pose = msg->pose;
 
            initialpose_pub_(new_initial_pose);
        }
 
        if (transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX)
        {
            geometry_msgs::msg::PoseStamped corrected_pose = *msg;
            if (gnss_offset_)
            {
                corrected_pose.pose.position.x = corrected_pose.pose.position.x + gnss_offset_->x();
                corrected_pose.pose.position.y = corrected_pose.pose.position.y + gnss_offset_->y();
                corrected_pose.pose.position.z = corrected_pose.pose.position.z + gnss_offset_->z();
            }
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Publishing GNSS pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_);
            current_pose_ = corrected_pose;
        }
    }

References current_pose_, localization_manager::DEGRADED_NO_LIDAR_FIX, localization_manager::LocalizationTransitionTable::getState(), gnss_offset_, localization_manager::INITIAL_POSE, initialpose_pub_, last_raw_gnss_value_, lidar_init_sequential_timesteps_counter_, localization_manager::LocalizationTransitionTable::signal(), transition_table_, and localization_manager::UNINITIALIZED.

Referenced by localization_manager::Node::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initialPoseCallback()

void localization_manager::LocalizationManager::initialPoseCallback ( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg )

Callback for the initial pose provided by Rviz or some external localization initializer.

Parameters

msg	The initial pose message

Definition at line 173 of file LocalizationManager.cpp.

    {
        lidar_init_sequential_timesteps_counter_ = 0;
        transition_table_.signal(LocalizationSignal::INITIAL_POSE);
        current_pose_ = boost::none; // Reset the current pose until a new pose is recieved
        initialpose_pub_(*msg);      // Forward the initial pose to the rest of the system
    }

References current_pose_, localization_manager::INITIAL_POSE, initialpose_pub_, lidar_init_sequential_timesteps_counter_, localization_manager::LocalizationTransitionTable::signal(), and transition_table_.

Referenced by localization_manager::Node::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nextId()

uint32_t localization_manager::LocalizationManager::nextId ( )

private

Generates the next id to be used for a timer.

Returns: The next available timer id

Definition at line 200 of file LocalizationManager.cpp.

    {
        next_id_++;
        return next_id_;
    }

References next_id_.

Referenced by stateTransitionCallback().

Here is the caller graph for this function:

◆ poseAndStatsCallback()

void localization_manager::LocalizationManager::poseAndStatsCallback	(	const geometry_msgs::msg::PoseStamped::ConstPtr	pose,
		const autoware_msgs::msg::NDTStat::ConstPtr	stats
	)

Synced callback for the NDT reported pose and status messages.

Parameters

pose	The pose reported by NDT matching of the vehicle in the map frame
stats	The stats reported by NDT matching for the accuracy of the provided pose

Definition at line 70 of file LocalizationManager.cpp.

    {
        double ndt_freq = computeNDTFreq(rclcpp::Time(pose->header.stamp, timer_clock_type_));
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Received pose resulting in frequency value of " << ndt_freq << " with score of " << stats->score);
 
        if (stats->score >= config_.fitness_score_fault_threshold || ndt_freq <= config_.ndt_frequency_fault_threshold)
        {
            transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE);
        }
        else if (stats->score >= config_.fitness_score_degraded_threshold || ndt_freq <= config_.ndt_frequency_degraded_threshold)
        {
            transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE);
        }
        else
        {
            // In GNSS_WITH_NDT_INIT mode, we shouldn't switch back to NDT and only rely on GPS
            if (!(static_cast<LocalizerMode>(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT &&
                  transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX &&
                  lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation))
            {
                transition_table_.signal(LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE);
            }
        }
 
        const LocalizationState state = transition_table_.getState();
 
        if (static_cast<LocalizerMode>(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && state == LocalizationState::OPERATIONAL && last_raw_gnss_value_)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_);
            if (lidar_init_sequential_timesteps_counter_ < config_.sequential_timesteps_until_gps_operation)
            {
                if (is_sequential_)
                    lidar_init_sequential_timesteps_counter_++;
 
                is_sequential_ = true;
            }
            else
            {
                transition_table_.signal(LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Switched to LIDAR_INITIALIZED_SWITCH_TO_GPS at lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_
                                                                                                                                                                            << ", with new state: " << transition_table_.getState());
            }
        }
        else if (!(static_cast<LocalizerMode>(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT &&
                   state == LocalizationState::DEGRADED_NO_LIDAR_FIX &&
                   last_raw_gnss_value_ &&
                   lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) // don't reset counter if Lidar initialized and switched to GPS
                                                                                                                  // as it will reset state to OPERATIONAL
        {
            is_sequential_ = false;
            lidar_init_sequential_timesteps_counter_ = 0;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState());
        }
 
        if (state == LocalizationState::OPERATIONAL && last_raw_gnss_value_)
        {
            tf2::Vector3 ndt_translation(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z);
 
            tf2::Vector3 gnss_translation(last_raw_gnss_value_->pose.position.x, last_raw_gnss_value_->pose.position.y, last_raw_gnss_value_->pose.position.z);
 
            gnss_offset_ = ndt_translation - gnss_translation;
        }
 
        if (state != LocalizationState::DEGRADED_NO_LIDAR_FIX)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Publishing mixed pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", and state" << state);
            current_pose_ = *pose;
        }
 
        prev_ndt_stamp_ = pose->header.stamp;
    }

Here is the call graph for this function:

◆ posePubTick()

void localization_manager::LocalizationManager::posePubTick ( )

Timer callback that controls the publication of the selected pose and status report.

Definition at line 263 of file LocalizationManager.cpp.

    {
        // Clear any expired timers
        clearTimers();
 
        // Evaluate NDT Frequency if we have started receiving messages
        // This check provides protection against excessively long NDT computation times that do not trigger the callback
        if (prev_ndt_stamp_)
        {
            double freq = computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), timer_factory_->now());
            if (freq <= config_.ndt_frequency_fault_threshold)
            {
                transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE);
            }
            else if (freq <= config_.ndt_frequency_degraded_threshold)
            {
                transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE);
            }
        }
 
        // check if last gnss time stamp is older than threshold and send the corresponding signal
        if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6))
        {
            transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT);
        }
 
        // Used in LocalizerMode::GNSS_WITH_NDT_INIT
        // If the state is not Operational with good NDT, or already using GPS only, we reset the counter
        if (transition_table_.getState() != LocalizationState::OPERATIONAL &&
            transition_table_.getState() != LocalizationState::DEGRADED_NO_LIDAR_FIX)
        {
            lidar_init_sequential_timesteps_counter_ = 0;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState());
        }
 
        // Publish current pose message if available
        if (current_pose_)
        {
            auto pose_to_publish = *current_pose_;
            if (static_cast<LocalizerMode>(config_.localization_mode) == LocalizerMode::GNSS_WITH_FIXED_OFFSET)
            {
                pose_to_publish.pose.position.x += config_.x_offset;
                pose_to_publish.pose.position.y += config_.y_offset;
                pose_to_publish.pose.position.z += config_.z_offset;
            }
            pose_pub_(pose_to_publish);
        }
 
        // Create and publish status report message
        carma_localization_msgs::msg::LocalizationStatusReport msg = stateToMsg(transition_table_.getState(), timer_factory_->now());
        state_pub_(msg);
    }

Referenced by localization_manager::Node::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setConfig()

void localization_manager::LocalizationManager::setConfig ( const LocalizationManagerConfig & config )

Set config.

Parameters

config localization manager config

Definition at line 46 of file LocalizationManager.cpp.

    {
        config_ = config;
    }

References config_.

◆ stateTransitionCallback()

void localization_manager::LocalizationManager::stateTransitionCallback	(	LocalizationState	prev_state,
		LocalizationState	new_state,
		LocalizationSignal	signal
	)

Callback for when a new state was triggered. Allows creation of entry/exit behavior for states.

Parameters

prev_state	The previous state
new_state	The new current state. This should never equal prev_state.
signal	The signal that triggered the state transition

Definition at line 227 of file LocalizationManager.cpp.

    {
        RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "State transition from " << prev_state << " to " << new_state << " with signal " << signal);
        // Mark the expired timers as expired if any
        if (current_timer_)
        {
            // NOTE unit testing of current algorithm depends on all timers being one-shot timers.
            timers_[current_timer_id_].second = true;
        }
 
        switch (new_state)
        {
        case LocalizationState::INITIALIZING:
            gnss_offset_ = boost::none;
            prev_ndt_stamp_ = boost::none;
 
            current_timer_id_ = nextId();
            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6),
                                                        std::bind(&LocalizationManager::timerCallback, this, new_state), true, true);
 
            timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id
            break;
        case LocalizationState::DEGRADED_NO_LIDAR_FIX:
            current_timer_id_ = nextId();
            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6),
                                                        std::bind(&LocalizationManager::timerCallback, this, new_state), true, true);
 
            timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id
 
            break;
        default:
            break;
        }
    }

References localization_manager::LocalizationManagerConfig::auto_initialization_timeout, config_, current_timer_, current_timer_id_, localization_manager::DEGRADED_NO_LIDAR_FIX, gnss_offset_, localization_manager::LocalizationManagerConfig::gnss_only_operation_timeout, localization_manager::INITIALIZING, nextId(), prev_ndt_stamp_, timer_factory_, timerCallback(), and timers_.

Referenced by LocalizationManager().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ systemAlertCallback()

void localization_manager::LocalizationManager::systemAlertCallback ( const carma_msgs::msg::SystemAlert::SharedPtr alert )

Callback for SystemAlert data. Used to check for lidar failure.

Parameters

alert The alert message to evaluate

Definition at line 181 of file LocalizationManager.cpp.

    {
        if (LIDAR_FAILURE_STRINGS.find(alert->description) != LIDAR_FAILURE_STRINGS.end())
        {
            transition_table_.signal(LocalizationSignal::LIDAR_SENSOR_FAILURE);
        }
    }

References LIDAR_FAILURE_STRINGS, localization_manager::LIDAR_SENSOR_FAILURE, localization_manager::LocalizationTransitionTable::signal(), and transition_table_.

Referenced by localization_manager::Node::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ timerCallback()

void localization_manager::LocalizationManager::timerCallback ( const LocalizationState origin_state )

Callback for timeouts. Used to trigger timeout signals for the state machine.

Parameters

origin_state The state that was the current state when the timer that triggered this callback was setup

Definition at line 189 of file LocalizationManager.cpp.

    {
        // If there is already a timer callback in progress or the expected state has changed then return
        if (origin_state != transition_table_.getState())
        {
            return;
        }
 
        transition_table_.signal(LocalizationSignal::TIMEOUT);
    }

References localization_manager::LocalizationTransitionTable::getState(), localization_manager::LocalizationTransitionTable::signal(), localization_manager::TIMEOUT, and transition_table_.

Referenced by stateTransitionCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

LocalizationManagerConfig localization_manager::LocalizationManager::config_

private

Definition at line 144 of file LocalizationManager.hpp.

Referenced by computeNDTFreq(), poseAndStatsCallback(), posePubTick(), setConfig(), and stateTransitionCallback().

◆ current_pose_

boost::optional<geometry_msgs::msg::PoseStamped> localization_manager::LocalizationManager::current_pose_

private

Definition at line 154 of file LocalizationManager.hpp.

Referenced by gnssPoseCallback(), initialPoseCallback(), poseAndStatsCallback(), and posePubTick().

◆ current_timer_

TimerUniquePtr localization_manager::LocalizationManager::current_timer_

private

Definition at line 163 of file LocalizationManager.hpp.

Referenced by stateTransitionCallback().

◆ current_timer_id_

uint32_t localization_manager::LocalizationManager::current_timer_id_

private

Definition at line 165 of file LocalizationManager.hpp.

Referenced by stateTransitionCallback().

◆ gnss_offset_

boost::optional<tf2::Vector3> localization_manager::LocalizationManager::gnss_offset_

private

Definition at line 153 of file LocalizationManager.hpp.

Referenced by gnssPoseCallback(), poseAndStatsCallback(), and stateTransitionCallback().

◆ initialpose_pub_

ManagedInitialPosePublisher localization_manager::LocalizationManager::initialpose_pub_

private

Definition at line 142 of file LocalizationManager.hpp.

Referenced by gnssPoseCallback(), and initialPoseCallback().

◆ is_sequential_

bool localization_manager::LocalizationManager::is_sequential_ = false

private

Definition at line 151 of file LocalizationManager.hpp.

Referenced by poseAndStatsCallback().

◆ last_raw_gnss_value_

boost::optional<geometry_msgs::msg::PoseStamped> localization_manager::LocalizationManager::last_raw_gnss_value_

private

Definition at line 152 of file LocalizationManager.hpp.

Referenced by gnssPoseCallback(), poseAndStatsCallback(), and posePubTick().

◆ LIDAR_FAILURE_STRINGS

const std::unordered_set< std::string > localization_manager::LocalizationManager::LIDAR_FAILURE_STRINGS

staticprivate

The set of strings which mark a lidar failure in a system alert message.

Definition at line 138 of file LocalizationManager.hpp.

Referenced by systemAlertCallback().

◆ lidar_init_sequential_timesteps_counter_

int localization_manager::LocalizationManager::lidar_init_sequential_timesteps_counter_ = 0

private

Definition at line 150 of file LocalizationManager.hpp.

Referenced by gnssPoseCallback(), initialPoseCallback(), poseAndStatsCallback(), and posePubTick().

◆ logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr localization_manager::LocalizationManager::logger_

private

Definition at line 157 of file LocalizationManager.hpp.

◆ mutex_

std::mutex localization_manager::LocalizationManager::mutex_

private

Definition at line 160 of file LocalizationManager.hpp.

Referenced by clearTimers().

◆ next_id_

uint32_t localization_manager::LocalizationManager::next_id_ = 0

private

Definition at line 164 of file LocalizationManager.hpp.

Referenced by nextId().

◆ pose_pub_

PosePublisher localization_manager::LocalizationManager::pose_pub_

private

Definition at line 140 of file LocalizationManager.hpp.

Referenced by posePubTick().

◆ prev_ndt_stamp_

boost::optional<rclcpp::Time> localization_manager::LocalizationManager::prev_ndt_stamp_

private

Definition at line 148 of file LocalizationManager.hpp.

Referenced by computeNDTFreq(), poseAndStatsCallback(), posePubTick(), and stateTransitionCallback().

◆ state_pub_

StatePublisher localization_manager::LocalizationManager::state_pub_

private

Definition at line 141 of file LocalizationManager.hpp.

Referenced by posePubTick().

◆ timer_clock_type_

rcl_clock_type_t localization_manager::LocalizationManager::timer_clock_type_ = RCL_SYSTEM_TIME

private

Definition at line 166 of file LocalizationManager.hpp.

Referenced by LocalizationManager(), computeNDTFreq(), poseAndStatsCallback(), and posePubTick().

◆ timer_factory_

std::unique_ptr<carma_ros2_utils::timers::TimerFactory> localization_manager::LocalizationManager::timer_factory_

private

Definition at line 161 of file LocalizationManager.hpp.

Referenced by LocalizationManager(), posePubTick(), and stateTransitionCallback().

◆ timers_

std::unordered_map<uint32_t, std::pair<TimerUniquePtr, bool> > localization_manager::LocalizationManager::timers_

private

Definition at line 162 of file LocalizationManager.hpp.

Referenced by clearTimers(), and stateTransitionCallback().

◆ transition_table_

LocalizationTransitionTable localization_manager::LocalizationManager::transition_table_

private

Definition at line 146 of file LocalizationManager.hpp.

Referenced by LocalizationManager(), getState(), gnssPoseCallback(), initialPoseCallback(), poseAndStatsCallback(), posePubTick(), systemAlertCallback(), and timerCallback().

The documentation for this class was generated from the following files:

localization_manager/include/localization_manager/LocalizationManager.hpp
localization_manager/src/LocalizationManager.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Typedef Documentation

◆ ManagedInitialPosePublisher

◆ PosePublisher

◆ StatePublisher

◆ TimerUniquePtr

Constructor & Destructor Documentation

◆ LocalizationManager()

Member Function Documentation

◆ clearTimers()

◆ computeFreq()

◆ computeNDTFreq()

◆ getState()

◆ gnssPoseCallback()

◆ initialPoseCallback()

◆ nextId()

◆ poseAndStatsCallback()

◆ posePubTick()

◆ setConfig()

◆ stateTransitionCallback()

◆ systemAlertCallback()

◆ timerCallback()

Member Data Documentation

◆ config_

◆ current_pose_

◆ current_timer_

◆ current_timer_id_

◆ gnss_offset_

◆ initialpose_pub_

◆ is_sequential_

◆ last_raw_gnss_value_

◆ LIDAR_FAILURE_STRINGS

◆ lidar_init_sequential_timesteps_counter_

◆ logger_

◆ mutex_

◆ next_id_

◆ pose_pub_

◆ prev_ndt_stamp_

◆ state_pub_

◆ timer_clock_type_

◆ timer_factory_

◆ timers_

◆ transition_table_