autoware_localization_util

Overview

autoware_localization_util is a collection of localization utility packages. It contains 5 individual library that used by autoware localization nodes.

• covariance_ellipse 2d covariance visualization wrapper.
• smart_pose_buffer pose buffer management library which contains interpolate and data validation.
• tree_structured_parzen_estimator A Tree Structured Parzen Estimator library.
• util_func A tool library which contains several function for localization nodes.

Design

• covariance_ellipse Translate geometry_msgs::msg::PoseWithCovariance message into ellipse visual marker to demonstrate covariance distribution.
• smart_pose_buffer A buffer library which implements pose message buffering, pose interpolate and pose validation.
• tree_structured_parzen_estimator A Probability Estimator library that includes estimator and log likelihood ratio calculation.
• util_func Tool function collection.

Usage

covariance_ellipse

Include header file to use:

#include "autoware/localization_util/covariance_ellipse.hpp"

calculate ellipse and visualize

autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);

  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
    ellipse_, input_msg->header, input_msg->pose);

smart_pose_buffer

buffer init

#include "autoware/localization_util/smart_pose_buffer.hpp"

using autoware::localization_util::SmartPoseBuffer;

std::unique_ptr<autoware::localization_util::SmartPoseBuffer> initial_pose_buffer_;
initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
    this->get_logger(), param_.validation.initial_pose_timeout_sec,
    param_.validation.initial_pose_distance_tolerance_m);

interpolate and pop out old pose message

std::optional<SmartPoseBuffer::InterpolateResult> interpolation_result_opt =
initial_pose_buffer_->interpolate(sensor_ros_time);

...

initial_pose_buffer_->pop_old(sensor_ros_time);
const SmartPoseBuffer::InterpolateResult & interpolation_result =
interpolation_result_opt.value();

clear buffer

initial_pose_buffer_->clear();

tree_structured_parzen_estimator

init the estimator. n_startup_trials – The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than ‘initial_estimate_particles_num’ and more than 0. If it is equal to ‘initial_estimate_particles_num’, the search will be the same as a full random search.

#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"

using autoware::localization_util::TreeStructuredParzenEstimator;

TreeStructuredParzenEstimator tpe(
TreeStructuredParzenEstimator::Direction::MAXIMIZE,
param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);

get estimation result

const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();

add new data to the estimator

```cpp TreeStructuredParzenEstimator::Input result(6); result[0] = pose.position.x; result[1] = pose.position.y; result[2] = pose.position.z; result[3] = rpy.x;

File truncated at 100 lines see the full file

Changelog for package autoware_localization_util

1.1.0 (2025-05-01)

• test(autoware_localization_util): add tests (#308)

  • test(autoware_localization_util): add tests for missed lines
  • style(pre-commit): autofix
  • fix(autoware_localization_util): fix pre-commit failure

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

• Contributors: NorahXiong

1.0.0 (2025-03-31)

• fix(autoware_localization_util): add missing test dependencies to autoware_localization_util (#293) Add missing test dependencies ament_index_cpp and rcl_yaml_param_parser Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>
• Contributors: Shane Loretz

0.3.0 (2025-03-21)

• chore: rename from [autoware.core]{.title-ref} to [autoware_core]{.title-ref} (#290)
• Contributors: Yutaka Kondo

0.2.0 (2025-02-07)

• unify version to 0.1.0
• update changelog
• feat: migrate autoware_localization_util from universe to core (#150) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro <<shintaro.sakoda@tier4.jp>> Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>
• Contributors: Yutaka Kondo, 心刚
• feat: migrate autoware_localization_util from universe to core (#150) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro <<shintaro.sakoda@tier4.jp>> Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>
• Contributors: 心刚

0.0.0 (2024-12-02)