localization_manager::Node Class Reference

Core execution node for this package. More...

#include <localization_manager_node.hpp>

Inheritance diagram for localization_manager::Node:

Collaboration diagram for localization_manager::Node:

Public Member Functions
	Node (const rclcpp::NodeOptions &)
	Node constructor. More...
void	publishPoseStamped (const geometry_msgs::msg::PoseStamped &msg) const
	Callback to publish the selected pose. More...
void	publishStatus (const carma_localization_msgs::msg::LocalizationStatusReport &msg) const
	Callback to publish the provided localization status report. More...
void	publishManagedInitialPose (const geometry_msgs::msg::PoseWithCovarianceStamped &msg) const
	Callback to publish the initial pose deemed suitable to intialize NDT. More...
void	poseAndStatsCallback (const geometry_msgs::msg::PoseStamped::ConstPtr pose, const autoware_msgs::msg::NDTStat::ConstPtr stats)
	Synchronized callback for pose and stats data for usage with message_filters. Provides exception handling. More...
rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Callback for dynamic parameter updates. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

Private Types
typedef message_filters::TimeSynchronizer< geometry_msgs::msg::PoseStamped, autoware_msgs::msg::NDTStat >	TimeSynchronizer

Private Attributes
message_filters::Subscriber< geometry_msgs::msg::PoseStamped, Node >	ndt_pose_sub_
message_filters::Subscriber< autoware_msgs::msg::NDTStat, Node >	ndt_score_sub_
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped >	gnss_pose_sub_
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseWithCovarianceStamped >	initialpose_sub_
carma_ros2_utils::SubPtr< carma_msgs::msg::SystemAlert >	system_alert_sub_
carma_ros2_utils::PubPtr< geometry_msgs::msg::PoseStamped >	pose_pub_
carma_ros2_utils::PubPtr< carma_localization_msgs::msg::LocalizationStatusReport >	state_pub_
carma_ros2_utils::PubPtr< geometry_msgs::msg::PoseWithCovarianceStamped >	managed_initial_pose_pub_
rclcpp::TimerBase::SharedPtr	pose_timer_
LocalizationManagerConfig	config_
std::unique_ptr< LocalizationManager >	manager_
std::shared_ptr< TimeSynchronizer >	pose_stats_synchronizer_

Detailed Description

Core execution node for this package.

Definition at line 45 of file localization_manager_node.hpp.

Member Typedef Documentation

TimeSynchronizer

typedef message_filters::TimeSynchronizer<geometry_msgs::msg::PoseStamped, autoware_msgs::msg::NDTStat> localization_manager::Node::TimeSynchronizer

private

Definition at line 70 of file localization_manager_node.hpp.

Constructor & Destructor Documentation

Node()

localization_manager::Node::Node ( const rclcpp::NodeOptions &  options )

explicit

Node constructor.

Definition at line 25 of file localization_manager_node.cpp.

        : carma_ros2_utils::CarmaLifecycleNode(options)
 
    {
        // Create initial config
        config_ = LocalizationManagerConfig();
 
        // Declare parameters
        config_.fitness_score_degraded_threshold = declare_parameter<double>("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold);
        config_.fitness_score_fault_threshold = declare_parameter<double>("fitness_score_fault_threshold", config_.fitness_score_fault_threshold);
        config_.ndt_frequency_degraded_threshold = declare_parameter<double>("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold);
        config_.ndt_frequency_fault_threshold = declare_parameter<double>("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold);
        config_.auto_initialization_timeout = declare_parameter<int>("auto_initialization_timeout", config_.auto_initialization_timeout);
        config_.gnss_only_operation_timeout = declare_parameter<int>("gnss_only_operation_timeout", config_.gnss_only_operation_timeout);
        config_.sequential_timesteps_until_gps_operation = declare_parameter<int>("sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation);
        config_.gnss_data_timeout = declare_parameter<int>("gnss_data_timeout", config_.gnss_data_timeout);
        config_.localization_mode = declare_parameter<int>("localization_mode", config_.localization_mode);
        config_.pose_pub_rate = declare_parameter<double>("pose_pub_rate", config_.pose_pub_rate);
        config_.x_offset = declare_parameter<double>("x_offset", config_.x_offset);
        config_.y_offset = declare_parameter<double>("y_offset", config_.y_offset);
        config_.z_offset = declare_parameter<double>("z_offset", config_.z_offset);
    }

References localization_manager::LocalizationManagerConfig::auto_initialization_timeout, config_, localization_manager::LocalizationManagerConfig::fitness_score_degraded_threshold, localization_manager::LocalizationManagerConfig::fitness_score_fault_threshold, localization_manager::LocalizationManagerConfig::gnss_data_timeout, localization_manager::LocalizationManagerConfig::gnss_only_operation_timeout, localization_manager::LocalizationManagerConfig::localization_mode, localization_manager::LocalizationManagerConfig::ndt_frequency_degraded_threshold, localization_manager::LocalizationManagerConfig::ndt_frequency_fault_threshold, localization_manager::LocalizationManagerConfig::pose_pub_rate, localization_manager::LocalizationManagerConfig::sequential_timesteps_until_gps_operation, localization_manager::LocalizationManagerConfig::x_offset, localization_manager::LocalizationManagerConfig::y_offset, and localization_manager::LocalizationManagerConfig::z_offset.

Member Function Documentation

handle_on_configure()

carma_ros2_utils::CallbackReturn localization_manager::Node::handle_on_configure

(

const rclcpp_lifecycle::State &

)

Definition at line 48 of file localization_manager_node.cpp.

    {
        // Reset config
        config_ = LocalizationManagerConfig();
 
        // Load parameters
        get_parameter<double>("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold);
        get_parameter<double>("fitness_score_fault_threshold", config_.fitness_score_fault_threshold);
        get_parameter<double>("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold);
        get_parameter<double>("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold);
        get_parameter<int>("auto_initialization_timeout", config_.auto_initialization_timeout);
        get_parameter<int>("gnss_only_operation_timeout", config_.gnss_only_operation_timeout);
        get_parameter<int>("sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation);
        get_parameter<int>("gnss_data_timeout", config_.gnss_data_timeout);
        get_parameter<int>("localization_mode", config_.localization_mode);
        get_parameter<double>("pose_pub_rate", config_.pose_pub_rate);
        get_parameter<double>("x_offset", config_.x_offset);
        get_parameter<double>("y_offset", config_.y_offset);
        get_parameter<double>("z_offset", config_.z_offset);
 
        RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "Loaded params: ");
 
        // Initialize worker object
        manager_.reset(new LocalizationManager(std::bind(&Node::publishPoseStamped, this, std_ph::_1),
                                               std::bind(&Node::publishStatus, this, std_ph::_1),
                                               std::bind(&Node::publishManagedInitialPose, this, std_ph::_1),
                                               config_,
                                               get_node_logging_interface(),
                                               std::make_unique<carma_ros2_utils::timers::ROSTimerFactory>(shared_from_this())));
 
        // Register runtime parameter update callback
        add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1));
 
        // Setup subscribers
        gnss_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("gnss_pose", 5,
                                                                              std::bind(&LocalizationManager::gnssPoseCallback, manager_.get(), std_ph::_1));
        initialpose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 1,
                                                                                              std::bind(&LocalizationManager::initialPoseCallback, manager_.get(), std_ph::_1));
 
        // Setup synchronized message_filters subscribers
        rclcpp::QoS qos(5);
        ndt_pose_sub_.subscribe(this, "ndt_pose", qos.get_rmw_qos_profile());
        ndt_score_sub_.subscribe(this, "ndt_stat", qos.get_rmw_qos_profile());
 
        pose_stats_synchronizer_ = std::make_shared<TimeSynchronizer>(ndt_pose_sub_, ndt_score_sub_, 5);
        pose_stats_synchronizer_->registerCallback(std::bind(&Node::poseAndStatsCallback, this, std_ph::_1, std_ph::_2));
 
        // system_alert_topic_ protected member of CarmaLifecycleNode
        system_alert_sub_ = create_subscription<carma_msgs::msg::SystemAlert>(system_alert_topic_, 1,
                                                                              std::bind(&LocalizationManager::systemAlertCallback, manager_.get(), std_ph::_1));
        // Setup publishers
        pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("selected_pose", 5);
        state_pub_ = create_publisher<carma_localization_msgs::msg::LocalizationStatusReport>("localization_status", 5);
 
        // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too
        rclcpp::PublisherOptions intra_proc_disabled;
        intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object
 
        auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // A publisher with this QoS will store all messages that it has sent on the topic
        pub_qos_transient_local.transient_local();                       // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers
                                                                         // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner.
        managed_initial_pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("managed_initialpose", pub_qos_transient_local, intra_proc_disabled);
 
        // Setup timer
        pose_timer_ = create_timer(get_clock(), std::chrono::milliseconds(int(1 / config_.pose_pub_rate * 1000)),
                                   std::bind(&LocalizationManager::posePubTick, manager_.get()));
 
        // Return success if everything initialized successfully
        return CallbackReturn::SUCCESS;
    }

References localization_manager::LocalizationManagerConfig::auto_initialization_timeout, config_, localization_manager::LocalizationManagerConfig::fitness_score_degraded_threshold, localization_manager::LocalizationManagerConfig::fitness_score_fault_threshold, localization_manager::LocalizationManagerConfig::gnss_data_timeout, localization_manager::LocalizationManagerConfig::gnss_only_operation_timeout, gnss_pose_sub_, localization_manager::LocalizationManager::gnssPoseCallback(), initialpose_sub_, localization_manager::LocalizationManager::initialPoseCallback(), localization_manager::LocalizationManagerConfig::localization_mode, managed_initial_pose_pub_, manager_, localization_manager::LocalizationManagerConfig::ndt_frequency_degraded_threshold, localization_manager::LocalizationManagerConfig::ndt_frequency_fault_threshold, ndt_pose_sub_, ndt_score_sub_, parameter_update_callback(), pose_pub_, localization_manager::LocalizationManagerConfig::pose_pub_rate, pose_stats_synchronizer_, pose_timer_, poseAndStatsCallback(), localization_manager::LocalizationManager::posePubTick(), publishManagedInitialPose(), publishPoseStamped(), publishStatus(), localization_manager::LocalizationManagerConfig::sequential_timesteps_until_gps_operation, state_pub_, system_alert_sub_, localization_manager::LocalizationManager::systemAlertCallback(), localization_manager::LocalizationManagerConfig::x_offset, localization_manager::LocalizationManagerConfig::y_offset, and localization_manager::LocalizationManagerConfig::z_offset.

Here is the call graph for this function:

parameter_update_callback()

rcl_interfaces::msg::SetParametersResult localization_manager::Node::parameter_update_callback

(

const std::vector< rclcpp::Parameter > &

parameters

)

Callback for dynamic parameter updates.

Definition at line 129 of file localization_manager_node.cpp.

    {
        auto error = update_params<double>(
        {{"pose_pub_rate", config_.pose_pub_rate},
        {"fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold},
        {"fitness_score_fault_threshold", config_.fitness_score_fault_threshold},
        {"ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold},
        {"ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold},
        {"x_offset", config_.x_offset},
        {"y_offset", config_.y_offset},
        {"z_offset", config_.z_offset}}, parameters);
        
        auto error_2 = update_params<int>(
        {{"auto_initialization_timeout", config_.auto_initialization_timeout},
        {"gnss_only_operation_timeout", config_.gnss_only_operation_timeout},
        {"gnss_data_timeout", config_.gnss_data_timeout},
        {"sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation}
        }, parameters);
 
        rcl_interfaces::msg::SetParametersResult result;
 
        result.successful = !error && !error_2;
 
        if (result.successful)
        {
            manager_->setConfig(config_);
        }
 
        return result;
    }

References localization_manager::LocalizationManagerConfig::auto_initialization_timeout, config_, localization_manager::LocalizationManagerConfig::fitness_score_degraded_threshold, localization_manager::LocalizationManagerConfig::fitness_score_fault_threshold, localization_manager::LocalizationManagerConfig::gnss_data_timeout, localization_manager::LocalizationManagerConfig::gnss_only_operation_timeout, manager_, localization_manager::LocalizationManagerConfig::ndt_frequency_degraded_threshold, localization_manager::LocalizationManagerConfig::ndt_frequency_fault_threshold, localization_manager::LocalizationManagerConfig::pose_pub_rate, localization_manager::LocalizationManagerConfig::sequential_timesteps_until_gps_operation, localization_manager::LocalizationManagerConfig::x_offset, localization_manager::LocalizationManagerConfig::y_offset, and localization_manager::LocalizationManagerConfig::z_offset.

Referenced by handle_on_configure().

Here is the caller graph for this function:

poseAndStatsCallback()

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

Synchronized callback for pose and stats data for usage with message_filters. Provides exception handling.

Parameters

pose	The received pose message
stats	The received stats message

Definition at line 165 of file localization_manager_node.cpp.

    {
        try
        {
            manager_->poseAndStatsCallback(pose, stats);
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("localization_manager"), "Uncaught Exception in localization_manager. Exception: " << e.what());
            handle_primary_state_exception(e);
        }
    }

References manager_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

publishManagedInitialPose()

void localization_manager::Node::publishManagedInitialPose

(

const geometry_msgs::msg::PoseWithCovarianceStamped &

msg

)

const

Callback to publish the initial pose deemed suitable to intialize NDT.

Parameters

msg	The msg to publish

Definition at line 160 of file localization_manager_node.cpp.

    {
        managed_initial_pose_pub_->publish(msg);
    }

References managed_initial_pose_pub_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

publishPoseStamped()

void localization_manager::Node::publishPoseStamped

(

const geometry_msgs::msg::PoseStamped &

msg

)

const

Callback to publish the selected pose.

Parameters

msg	The pose to publish

Definition at line 119 of file localization_manager_node.cpp.

    {
        pose_pub_->publish(msg);
    }

References pose_pub_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

publishStatus()

void localization_manager::Node::publishStatus

(

const carma_localization_msgs::msg::LocalizationStatusReport &

msg

)

const

Callback to publish the provided localization status report.

Parameters

msg	The report to publish

Definition at line 124 of file localization_manager_node.cpp.

    {
        state_pub_->publish(msg);
    }

References state_pub_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

config_

LocalizationManagerConfig localization_manager::Node::config_

private

Definition at line 64 of file localization_manager_node.hpp.

Referenced by Node(), handle_on_configure(), and parameter_update_callback().

gnss_pose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> localization_manager::Node::gnss_pose_sub_

private

Definition at line 51 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

initialpose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseWithCovarianceStamped> localization_manager::Node::initialpose_sub_

private

Definition at line 52 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

managed_initial_pose_pub_

carma_ros2_utils::PubPtr<geometry_msgs::msg::PoseWithCovarianceStamped> localization_manager::Node::managed_initial_pose_pub_

private

Definition at line 58 of file localization_manager_node.hpp.

Referenced by handle_on_configure(), and publishManagedInitialPose().

manager_

std::unique_ptr<LocalizationManager> localization_manager::Node::manager_

private

Definition at line 67 of file localization_manager_node.hpp.

Referenced by handle_on_configure(), parameter_update_callback(), and poseAndStatsCallback().

ndt_pose_sub_

message_filters::Subscriber<geometry_msgs::msg::PoseStamped, Node> localization_manager::Node::ndt_pose_sub_

private

Definition at line 49 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

ndt_score_sub_

message_filters::Subscriber<autoware_msgs::msg::NDTStat, Node> localization_manager::Node::ndt_score_sub_

private

Definition at line 50 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

pose_pub_

carma_ros2_utils::PubPtr<geometry_msgs::msg::PoseStamped> localization_manager::Node::pose_pub_

private

Definition at line 56 of file localization_manager_node.hpp.

Referenced by handle_on_configure(), and publishPoseStamped().

pose_stats_synchronizer_

std::shared_ptr<TimeSynchronizer> localization_manager::Node::pose_stats_synchronizer_

private

Definition at line 71 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

pose_timer_

rclcpp::TimerBase::SharedPtr localization_manager::Node::pose_timer_

private

Definition at line 61 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

state_pub_

carma_ros2_utils::PubPtr<carma_localization_msgs::msg::LocalizationStatusReport> localization_manager::Node::state_pub_

private

Definition at line 57 of file localization_manager_node.hpp.

Referenced by handle_on_configure(), and publishStatus().

system_alert_sub_

carma_ros2_utils::SubPtr<carma_msgs::msg::SystemAlert> localization_manager::Node::system_alert_sub_

private

Definition at line 53 of file localization_manager_node.hpp.

Referenced by handle_on_configure().

---

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

• localization_manager/include/localization_manager/localization_manager_node.hpp
• localization_manager/src/localization_manager_node.cpp