ROS index logo ROS Index
  1. Home
  2. Packages
  3. autoware_localization_util
 
No version for distro humble. Known supported distros are highlighted in the buttons above.
No version for distro jazzy. Known supported distros are highlighted in the buttons above.
No version for distro kilted. Known supported distros are highlighted in the buttons above.
No version for distro rolling. Known supported distros are highlighted in the buttons above.

autoware_localization_util package from autoware_core repo

autoware_core autoware_component_interface_specs autoware_geography_utils autoware_global_parameter_loader autoware_interpolation autoware_kalman_filter autoware_lanelet2_utils autoware_motion_utils autoware_node autoware_object_recognition_utils autoware_osqp_interface autoware_point_types autoware_qp_interface autoware_signal_processing autoware_trajectory autoware_vehicle_info_utils autoware_core_control autoware_simple_pure_pursuit autoware_core_localization autoware_ekf_localizer autoware_gyro_odometer autoware_localization_util autoware_ndt_scan_matcher autoware_pose_initializer autoware_stop_filter autoware_twist2accel autoware_core_map autoware_lanelet2_map_visualizer autoware_map_height_fitter autoware_map_loader autoware_map_projection_loader autoware_core_perception autoware_euclidean_cluster_object_detector autoware_ground_filter autoware_perception_objects_converter autoware_core_planning autoware_mission_planner autoware_objects_of_interest_marker_interface autoware_path_generator autoware_planning_factor_interface autoware_planning_topic_converter autoware_route_handler autoware_velocity_smoother autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_stop_line_module autoware_motion_velocity_obstacle_stop_module autoware_motion_velocity_planner autoware_motion_velocity_planner_common autoware_core_sensing autoware_crop_box_filter autoware_downsample_filters autoware_gnss_poser autoware_vehicle_velocity_converter autoware_planning_test_manager autoware_pyplot autoware_test_node autoware_test_utils autoware_testing autoware_core_vehicle

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 1.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/autowarefoundation/autoware_core.git
VCS Type git
VCS Version main
Last Updated 2025-06-07
Dev Status UNKNOWN
CI status No Continuous Integration
Released UNRELEASED
Tags planner ros calibration self-driving-car autonomous-driving autonomous-vehicles ros2 3d-map autoware
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Package Description

The autoware_localization_util package

Additional Links

No additional links.

Maintainers

  • Xingang Liu
  • Yamato Ando
  • Masahiro Sakamoto
  • Shintaro Sakoda
  • Kento Yabuuchi
  • NGUYEN Viet Anh
  • Taiki Yamada
  • Ryu Yamamoto

Authors

  • Yamato Ando
localization/autoware_localization_util/README.md

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

TreeStructuredParzenEstimator::Input result(6);
    result[0] = pose.position.x;
    result[1] = pose.position.y;
    result[2] = pose.position.z;
    result[3] = rpy.x;
    result[4] = rpy.y;
    result[5] = rpy.z;
tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});

util_func

include header file to use

#include "autoware/localization_util/util_func.hpp"

using autoware::localization_util::exchange_color_crc;
using autoware::localization_util::matrix4f_to_pose;
using autoware::localization_util::point_to_vector3d;
using autoware::localization_util::pose_to_matrix4f;

list of useful function

std_msgs::msg::ColorRGBA exchange_color_crc(double x);
double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
  const double r_rad, const double p_rad, const double y_rad);
geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
  const double r_deg, const double p_deg, const double y_deg);
geometry_msgs::msg::Twist calc_twist(
  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
geometry_msgs::msg::PoseStamped interpolate_pose(
  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
  const rclcpp::Time & time_stamp);
geometry_msgs::msg::PoseStamped interpolate_pose(
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
template <class T>
T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform);double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);

void output_pose_with_cov_to_log(
  const rclcpp::Logger & logger, const std::string & prefix,
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
CHANGELOG

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)

Wiki Tutorials

This package does not provide any links to tutorials in it's rosindex metadata. You can check on the ROS Wiki Tutorials page for the package.

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged autoware_localization_util at Robotics Stack Exchange

No version for distro noetic. Known supported distros are highlighted in the buttons above.
No version for distro galactic. Known supported distros are highlighted in the buttons above.
No version for distro iron. Known supported distros are highlighted in the buttons above.
No version for distro melodic. Known supported distros are highlighted in the buttons above.