feat: add localization utils (autowarefoundation#59)

* release v0.4.0 * Avoid setting CMAKE_BUILD_TYPE=Release in each CMakeLists.txt (autowarefoundation#720) * remove set CMAKE_BUILD_TYPE Release in each CMakeLists.txt * remove set CMAKE_BUILD_TYPE Release in ndt_pcl_modified * set compile options for debug in ndt_omp * Fix indent * add warning if -DCMAKE_BUILD_TYPE=Release is not set in ndt_omp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Port autoware_localization_srvs * Add DEPENDENCIES * Revert "remove ROS1 packages temporarily" This reverts commit ee01a755ac5b22c61f4274ae2ee57b0a18150a78. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * Port pose_initializer to ROS2 (#11) * CMakeLists.txt & package.xml * Compiles * Works * Use callback instead * Reviewer feedback * Add sequence number check * Review feedback * Fix lint issues (autowarefoundation#128) * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * fixing trasient_local in ROS2 packages (autowarefoundation#160) * [ekf_localizer] [pose_initializer] fix topic message type (autowarefoundation#176) Co-authored-by: Autoware <autoware@tier4.jp> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * ading linters to pose_initializer (autowarefoundation#186) * adding linters to autoware_localization_srvs (autowarefoundation#185) * Port initial-pose-button-panel (autowarefoundation#125) * Port Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org> * Initialize button when request received Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org> * Amend buildtool Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org> * Fix license Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org> * apply env_var to use_sim_time (autowarefoundation#222) * retry gnss initialize (autowarefoundation#1043) (autowarefoundation#263) Co-authored-by: YamatoAndo <yamato.ando@gmail.com> * Ros2 v0.8.0 initial pose buttion panel (autowarefoundation#312) * restore initial pose button panel files for v0.8.0 update * fix typos in localization (autowarefoundation#890) * Fix memory leak and remove unused properties (autowarefoundation#964) * Revert "restore initial pose button panel files for v0.8.0 update" This reverts commit 01281a2633636f80f241a788f9d35c517a2977e1. * delete unused header * add ament lint test * apply ament_lint * fix bug Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * modify pose_initialize for foa (autowarefoundation#382) * pose_initialize for foa (autowarefoundation#1171) * pose_initialize for foa Signed-off-by: Yamato ANDO <yamato.ando@gmail.com> * rename topic Signed-off-by: Yamato ANDO <yamato.ando@gmail.com> * fix topic name Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Fix missing gnss subscriber (autowarefoundation#417) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Ros2 fix ndt align srv (autowarefoundation#442) * Add success field to ndt align srv response Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Apply change of ndt align srv Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Apply change of ndt align srv Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix dependency type of rosidl_default_generators (autowarefoundation#1801) * Fix dependency type of rosidl_default_generators Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove unnecessary depends Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use ament_cmake_auto Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Add autoware api (autowarefoundation#1979) * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove COLCON_IGNORE under localization (autowarefoundation#553) * Remove 'pose_initializer/COLCON_IGNORE' (autowarefoundation#556) * add README.md (autowarefoundation#623) * add README.md * fix typo * minor fix * Update localization/util/autoware_localization_rviz_plugin/initial_pose_button_panel/README.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update localization/util/autoware_localization_rviz_plugin/initial_pose_button_panel/README.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update localization/util/autoware_localization_rviz_plugin/initial_pose_button_panel/README.md Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * add readme in pose initializer (autowarefoundation#636) * add readme in pose initializer Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * modify doc Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * move autoware_localization_srvs package (autowarefoundation#689) * move autoware_localization_srvs package * apply pre-commit * rename pose_with_cov to pose_with_covariance * apply pre-commit * fix: move initial_pose_button_panel directory Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Daichi Murakami <harihitode@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Nikolai Morin <nikolai.morin@apex.ai> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Autoware <autoware@tier4.jp> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
badai-nguyen · Dec 6, 2021 · cdbc61a · cdbc61a
1 parent e85544c
commit cdbc61a
Show file tree

Hide file tree

Showing 14 changed files with 748 additions and 0 deletions.
diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.5)
+project(initial_pose_button_panel)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+
+add_definitions(-DQT_NO_KEYWORDS -g)
+set(CMAKE_AUTOMOC ON)
+
+ament_auto_add_library(initial_pose_button_panel SHARED
+  src/initial_pose_button_panel.cpp)
+target_link_libraries(initial_pose_button_panel
+  ${QT_LIBRARIES})
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+if(BUILD_TESTING)
+find_package(ament_lint_auto REQUIRED)
+ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  plugins
+)
diff --git a/localization/initial_pose_button_panel/README.md b/localization/initial_pose_button_panel/README.md
@@ -0,0 +1,18 @@
+# initial_pose_button_panel
+
+## Role
+
+`initial_pose_button_panel` is the package to send a request to the localization module to calculate the current ego pose.
+
+It starts calculating the current ego pose by pushing the button on Rviz, implemented as an Rviz plugin.
+You can see the button on the right bottom of Rviz.
+
+![initialize_button](./media/initialize_button.png)
+
+## Input / Output
+
+### Input topics
+
+| Name                                           | Type                                          | Description                                                    |
+| ---------------------------------------------- | --------------------------------------------- | -------------------------------------------------------------- |
+| `/sensing/gnss/pose_with_covariance` (default) | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose with covariance to calculate the current ego pose |
diff --git a/localization/initial_pose_button_panel/media/initialize_button.png b/localization/initial_pose_button_panel/media/initialize_button.png
diff --git a/localization/initial_pose_button_panel/package.xml b/localization/initial_pose_button_panel/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>initial_pose_button_panel</name>
+  <version>0.1.0</version>
+  <description>The initial_pose_button_panel package</description>
+
+  <maintainer email="yamato.ando@gmail.com">Yamato ANDO</maintainer>
+  <author>Yamato ANDO</author>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <depend>autoware_localization_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rviz_common</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>
diff --git a/localization/initial_pose_button_panel/plugins/plugin_description.xml b/localization/initial_pose_button_panel/plugins/plugin_description.xml
@@ -0,0 +1,9 @@
+<library path="initial_pose_button_panel">
+    <class name="autoware_localization_rviz_plugin/InitialPoseButtonPanel"
+           type="autoware_localization_rviz_plugin::InitialPoseButtonPanel"
+           base_class_type="rviz_common::Panel">
+        <description>
+          initial button.
+        </description>
+    </class>
+</library>
diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp
@@ -0,0 +1,127 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "initial_pose_button_panel.hpp"
+
+#include <QFileDialog>
+#include <QHBoxLayout>
+#include <QLineEdit>
+#include <QPainter>
+#include <QPushButton>
+#include <pluginlib/class_list_macros.hpp>
+#include <rviz_common/display_context.hpp>
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace autoware_localization_rviz_plugin
+{
+InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  topic_label_ = new QLabel("PoseWithCovarianceStamped ");
+  topic_label_->setAlignment(Qt::AlignCenter);
+
+  topic_edit_ = new QLineEdit("/sensing/gnss/pose_with_covariance");
+  connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic()));
+
+  initialize_button_ = new QPushButton("Wait for subscribe topic");
+  initialize_button_->setEnabled(false);
+  connect(initialize_button_, SIGNAL(clicked(bool)), SLOT(pushInitializeButton()));
+
+  status_label_ = new QLabel("Not Initialize");
+  status_label_->setAlignment(Qt::AlignCenter);
+  status_label_->setStyleSheet("QLabel { background-color : gray;}");
+
+  QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+  initialize_button_->setSizePolicy(q_size_policy);
+
+  auto * topic_layout = new QHBoxLayout;
+  topic_layout->addWidget(topic_label_);
+  topic_layout->addWidget(topic_edit_);
+
+  auto * v_layout = new QVBoxLayout;
+  v_layout->addLayout(topic_layout);
+  v_layout->addWidget(initialize_button_);
+  v_layout->addWidget(status_label_);
+
+  setLayout(v_layout);
+}
+void InitialPoseButtonPanel::onInitialize()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+
+  pose_cov_sub_ = raw_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    topic_edit_->text().toStdString(), 10,
+    std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1));
+
+  client_ = raw_node->create_client<autoware_localization_msgs::srv::PoseWithCovarianceStamped>(
+    "/localization/util/initialize_pose");
+}
+
+void InitialPoseButtonPanel::callbackPoseCov(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
+{
+  pose_cov_msg_ = *msg;
+  initialize_button_->setText("Pose Initializer Let's GO!");
+  initialize_button_->setEnabled(true);
+}
+
+void InitialPoseButtonPanel::editTopic()
+{
+  pose_cov_sub_.reset();
+  rclcpp::Node::SharedPtr raw_node =
+    this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  pose_cov_sub_ = raw_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    topic_edit_->text().toStdString(), 10,
+    std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1));
+  initialize_button_->setText("Wait for subscribe topic");
+  initialize_button_->setEnabled(false);
+}
+
+void InitialPoseButtonPanel::pushInitializeButton()
+{
+  // lock button
+  initialize_button_->setEnabled(false);
+
+  status_label_->setStyleSheet("QLabel { background-color : dodgerblue;}");
+  status_label_->setText("Initializing...");
+
+  std::thread thread([this] {
+    auto req =
+      std::make_shared<autoware_localization_msgs::srv::PoseWithCovarianceStamped::Request>();
+    req->pose_with_covariance = pose_cov_msg_;
+
+    client_->async_send_request(
+      req,
+      [this](
+        rclcpp::Client<autoware_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture
+          result) {
+        status_label_->setStyleSheet("QLabel { background-color : lightgreen;}");
+        status_label_->setText("OK!!!");
+
+        // unlock button
+        initialize_button_->setEnabled(true);
+      });
+  });
+
+  thread.detach();
+}
+
+}  // end namespace autoware_localization_rviz_plugin
+
+PLUGINLIB_EXPORT_CLASS(
+  autoware_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel)
diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp
@@ -0,0 +1,65 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#ifndef INITIAL_POSE_BUTTON_PANEL_HPP_
+#define INITIAL_POSE_BUTTON_PANEL_HPP_
+
+#include <QLabel>
+#include <QLineEdit>
+#include <QPushButton>
+#include <QSettings>
+
+#include <string>
+#ifndef Q_MOC_RUN
+
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+#include <rviz_common/properties/ros_topic_property.hpp>
+#endif
+#include <autoware_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+namespace autoware_localization_rviz_plugin
+{
+class InitialPoseButtonPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit InitialPoseButtonPanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+  void callbackPoseCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
+
+public Q_SLOTS:
+  void editTopic();
+  void pushInitializeButton();
+
+protected:
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_cov_sub_;
+
+  rclcpp::Client<autoware_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr client_;
+
+  QLabel * topic_label_;
+  QLineEdit * topic_edit_;
+  QPushButton * initialize_button_;
+  QLabel * status_label_;
+
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_msg_;
+};
+
+}  // end namespace autoware_localization_rviz_plugin
+
+#endif  // INITIAL_POSE_BUTTON_PANEL_HPP_
diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+project(pose_initializer)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+find_package(PCL REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(pose_initializer
+  src/pose_initializer_node.cpp
+  src/pose_initializer_core.cpp
+)
+target_link_libraries(pose_initializer ${PCL_LIBRARIES})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md
@@ -0,0 +1,24 @@
+# pose_initializer
+
+## Purpose
+
+`pose_initializer` is the package to send an initial pose to `ekf_localizer`.
+It receives roughly estimated initial pose from GNSS/user.
+Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service.
+Finally, it publishes the initial pose to `ekf_localizer`.
+
+## Input / Output
+
+### Input topics
+
+| Name                                 | Type                                          | Description            |
+| ------------------------------------ | --------------------------------------------- | ---------------------- |
+| `/initialpose`                       | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose from rviz |
+| `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss         |
+| `/map/pointcloud_map`                | sensor_msgs::msg::PointCloud2                 | pointcloud map         |
+
+### Output topics
+
+| Name             | Type                                          | Description                 |
+| ---------------- | --------------------------------------------- | --------------------------- |
+| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |