feat: add localization utils

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
+)

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 |

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>

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>

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)

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_

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
+)

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 |