Autoware.Auto
autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires > Class Template Reference

#include <localization_node.hpp>

Inheritance diagram for autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >:
Collaboration diagram for autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >:

Public Types

using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped
 
using TransformStamped = geometry_msgs::msg::TransformStamped
 
using RegistrationSummary = localization_common::OptimizedRegistrationSummary
 

Public Member Functions

 RelativeLocalizerNode (const std::string &node_name, const std::string &name_space, const TopicQoS &observation_sub_config, const TopicQoS &map_sub_config, const TopicQoS &pose_pub_config, const TopicQoS &initial_pose_sub_config, const PoseInitializerT &pose_initializer, LocalizerPublishMode publish_tf=LocalizerPublishMode::NO_PUBLISH_TF)
 
 RelativeLocalizerNode (const std::string &node_name, const rclcpp::NodeOptions &options, const PoseInitializerT &pose_initializer)
 
 RelativeLocalizerNode (const std::string &node_name, const std::string &name_space, const PoseInitializerT &pose_initializer)
 
const rclcpp::Publisher< PoseWithCovarianceStamped >::ConstSharedPtr get_publisher ()
 Get a const pointer of the output publisher. Can be used for matching against subscriptions. More...
 

Static Public Attributes

static constexpr bool USE_DEDICATED_TF_THREAD {true}
 

Protected Member Functions

void set_localizer (std::unique_ptr< LocalizerT > &&localizer_ptr)
 
void set_map (std::unique_ptr< MapT > &&map_ptr)
 
virtual void on_bad_registration (std::exception_ptr eptr)
 Handle the exceptions during registration. More...
 
virtual void on_bad_map (std::exception_ptr eptr)
 Handle the exceptions during map setting. More...
 
void on_exception (std::exception_ptr eptr, const std::string &error_source)
 
virtual void on_observation_with_invalid_map (typename ObservationMsgT::ConstSharedPtr)
 Default behavior when an observation is received with no valid existing map. More...
 
virtual void on_invalid_output (const PoseWithCovarianceStamped &pose)
 
virtual bool validate_output (const RegistrationSummary &summary, const PoseWithCovarianceStamped &pose, const TransformStamped &guess)
 

Detailed Description

template<typename ObservationMsgT, typename MapMsgT, typename MapT, typename LocalizerT, typename PoseInitializerT, Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
class autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >

Base relative localizer node that publishes map->base_link relative transform messages for a given observation source and map.

Template Parameters
ObservationMsgTMessage type to register against a map.
MapMsgTMap type
LocalizerTLocalizer type.
PoseInitializerTPose initializer type.

Member Typedef Documentation

◆ PoseWithCovarianceStamped

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
using autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped

◆ RegistrationSummary

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
using autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::RegistrationSummary = localization_common::OptimizedRegistrationSummary

◆ TransformStamped

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
using autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::TransformStamped = geometry_msgs::msg::TransformStamped

Constructor & Destructor Documentation

◆ RelativeLocalizerNode() [1/3]

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::RelativeLocalizerNode ( const std::string &  node_name,
const std::string &  name_space,
const TopicQoS observation_sub_config,
const TopicQoS map_sub_config,
const TopicQoS pose_pub_config,
const TopicQoS initial_pose_sub_config,
const PoseInitializerT &  pose_initializer,
LocalizerPublishMode  publish_tf = LocalizerPublishMode::NO_PUBLISH_TF 
)
inline

Constructor

Parameters
node_nameName of node
name_spaceNamespace of node
observation_sub_configtopic and QoS setting for the observation subscription.
map_sub_configtopic and QoS setting for the map subscription.
pose_pub_configtopic and QoS setting for the output pose publisher.
initial_pose_sub_configtopic and QoS setting for the initialpose subscription.
pose_initializerPose initializer.
publish_tfWhether to publish to the tf topic. This can be used to publish transform messages when the relative localizer is the only source of localization.

◆ RelativeLocalizerNode() [2/3]

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::RelativeLocalizerNode ( const std::string &  node_name,
const rclcpp::NodeOptions &  options,
const PoseInitializerT &  pose_initializer 
)
inline

◆ RelativeLocalizerNode() [3/3]

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::RelativeLocalizerNode ( const std::string &  node_name,
const std::string &  name_space,
const PoseInitializerT &  pose_initializer 
)
inline

Constructor using ros parameters

Parameters
node_nameNode name
name_spaceNode namespace
pose_initializerPose initializer

Member Function Documentation

◆ get_publisher()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
const rclcpp::Publisher<PoseWithCovarianceStamped>::ConstSharedPtr autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::get_publisher ( )
inline

Get a const pointer of the output publisher. Can be used for matching against subscriptions.

◆ on_bad_map()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
virtual void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::on_bad_map ( std::exception_ptr  eptr)
inlineprotectedvirtual

Handle the exceptions during map setting.

◆ on_bad_registration()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
virtual void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::on_bad_registration ( std::exception_ptr  eptr)
inlineprotectedvirtual

Handle the exceptions during registration.

◆ on_exception()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::on_exception ( std::exception_ptr  eptr,
const std::string &  error_source 
)
inlineprotected

◆ on_invalid_output()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
virtual void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::on_invalid_output ( const PoseWithCovarianceStamped pose)
inlineprotectedvirtual

Default behavior when hte pose output is evaluated to be invalid.

Parameters
posePose output.

◆ on_observation_with_invalid_map()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
virtual void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::on_observation_with_invalid_map ( typename ObservationMsgT::ConstSharedPtr  )
inlineprotectedvirtual

Default behavior when an observation is received with no valid existing map.

◆ set_localizer()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::set_localizer ( std::unique_ptr< LocalizerT > &&  localizer_ptr)
inlineprotected

Set the localizer.

Parameters
localizer_ptrrvalue to the localizer to set.

◆ set_map()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
void autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::set_map ( std::unique_ptr< MapT > &&  map_ptr)
inlineprotected

◆ validate_output()

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires = traits::LocalizerConstraint<LocalizerT, ObservationMsgT, MapT>::value, Requires = traits::MapConstraint<MapT, MapMsgT>::value>
virtual bool autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, Requires, Requires >::validate_output ( const RegistrationSummary summary,
const PoseWithCovarianceStamped pose,
const TransformStamped guess 
)
inlineprotectedvirtual

Validate the pose estimate given the registration summary and the initial guess. This function by default returns true.

Parameters
summaryRegistration summary.
posePose estimate.
guessInitial guess.
Returns
True if the estimate is valid and can be published.

Member Data Documentation

◆ USE_DEDICATED_TF_THREAD

template<typename ObservationMsgT , typename MapMsgT , typename MapT , typename LocalizerT , typename PoseInitializerT , Requires R1, Requires R2>
constexpr bool autoware::localization::localization_nodes::RelativeLocalizerNode< ObservationMsgT, MapMsgT, MapT, LocalizerT, PoseInitializerT, R1, R2 >::USE_DEDICATED_TF_THREAD {true}
staticconstexpr

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