feat(yabloc): add camera and vector map localization

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Parameter files -->
+  <arg name="mode"/>
   <arg name="crop_box_filter_measurement_range_param_path"/>
   <arg name="voxel_grid_downsample_filter_param_path"/>
   <arg name="random_downsample_filter_param_path"/>
@@ -27,10 +28,14 @@
       </include>
     </group>
     <!-- pose_estimator module -->
-    <group>
+    <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
       <push-ros-namespace namespace="pose_estimator"/>
       <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_estimator/pose_estimator.launch.xml"/>
     </group>
+    <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera&quot;')">
+      <push-ros-namespace namespace="yabloc"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_estimator/yabloc.launch.xml"/>
+    </group>
     <!-- twist_estimator module -->
     <group>
       <push-ros-namespace namespace="twist_estimator"/>

launch/tier4_localization_launch/launch/pose_estimator/yabloc.launch.xml

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- source camera image topics -->
+  <arg name="src_image" default="/sensing/camera/traffic_light/image_raw/compressed"/>
+  <arg name="src_info" default="/sensing/camera/traffic_light/camera_info"/>
+
+  <!-- default parameter path -->
+  <!-- initializer -->
+  <arg name="camera_pose_initializer_param_path" default="$(find-pkg-share yabloc_pose_initializer)/config/camera_pose_initializer.param.yaml"/>
+  <!-- particle filter-->
+  <arg name="camera_particle_corrector_param_path" default="$(find-pkg-share yabloc_particle_filter)/config/camera_particle_corrector.param.yaml"/>
+  <arg name="gnss_particle_corrector_param_path" default="$(find-pkg-share yabloc_particle_filter)/config/gnss_particle_corrector.param.yaml"/>
+  <arg name="predictor_param_path" default="$(find-pkg-share yabloc_particle_filter)/config/predictor.param.yaml"/>
+  <!-- image processing -->
+  <arg name="graph_segment_param_path" default="$(find-pkg-share yabloc_image_processing)/config/graph_segment.param.yaml"/>
+  <arg name="segment_filter_param_path" default="$(find-pkg-share yabloc_image_processing)/config/segment_filter.param.yaml"/>
+  <arg name="undistort_param_path" default="$(find-pkg-share yabloc_image_processing)/config/undistort.param.yaml"/>
+  <!-- map -->
+  <arg name="ground_server_param_path" default="$(find-pkg-share yabloc_common)/config/ground_server.param.yaml"/>
+  <arg name="ll2_decomposer_param_path" default="$(find-pkg-share yabloc_common)/config/ll2_decomposer.param.yaml"/>
+
+  <let name="input_pose" value="/localization/pose_twist_fusion_filter/pose"/>
+
+  <!-- initializer -->
+  <group>
+    <push-ros-namespace namespace="initializer"/>
+    <include file="$(find-pkg-share yabloc_pose_initializer)/launch/yabloc_pose_initializer.launch.xml"/>
+  </group>
+
+  <!-- particle filter -->
+  <group>
+    <push-ros-namespace namespace="pf"/>
+    <include file="$(find-pkg-share yabloc_particle_filter)/launch/yabloc_particle_filter.launch.xml"/>
+  </group>
+
+  <!-- image processing -->
+  <group>
+    <push-ros-namespace namespace="image_processing"/>
+    <include file="$(find-pkg-share yabloc_image_processing)/launch/yabloc_image_processing.launch.xml">
+      <arg name="src_image" value="$(var src_image)"/>
+      <arg name="src_info" value="$(var src_info)"/>
+      <arg name="input_pose" value="$(var input_pose)"/>
+    </include>
+  </group>
+
+  <!-- map -->
+  <group>
+    <push-ros-namespace namespace="map"/>
+    <include file="$(find-pkg-share yabloc_common)/launch/yabloc_common.launch.xml">
+      <arg name="input_particle_pose" value="$(var input_pose)"/>
+    </include>
+  </group>
+</launch>

launch/tier4_localization_launch/package.xml

@@ -21,6 +21,10 @@
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
   <exec_depend>topic_tools</exec_depend>
+  <exec_depend>yabloc_common</exec_depend>
+  <exec_depend>yabloc_image_processing</exec_depend>
+  <exec_depend>yabloc_particle_filter</exec_depend>
+  <exec_depend>yabloc_pose_initializer</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

localization/pose_initializer/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_executable(pose_initializer_node
   src/pose_initializer/pose_initializer_core.cpp
   src/pose_initializer/gnss_module.cpp
   src/pose_initializer/ndt_module.cpp
+  src/pose_initializer/yabloc_module.cpp
   src/pose_initializer/stop_check_module.cpp
   src/pose_initializer/ekf_localization_trigger_module.cpp
   src/pose_initializer/ndt_localization_trigger_module.cpp

localization/pose_initializer/config/pose_initializer.param.yaml

@@ -3,4 +3,5 @@
     gnss_enabled: true
     ndt_enabled: true
     ekf_enabled: true
+    yabloc_enabled: false
     stop_check_enabled: true

localization/pose_initializer/launch/pose_initializer.launch.xml

@@ -6,6 +6,7 @@
   <node pkg="pose_initializer" exec="pose_initializer_node" name="pose_initializer_node">
     <param from="$(var common_config_file)"/>
     <param from="$(var config_file)"/>
+    <remap from="yabloc_align" to="/localization/yabloc/initializer/yabloc_align_srv"/>
     <remap from="ndt_align" to="/localization/pose_estimator/ndt_align_srv"/>
     <remap from="stop_check_twist" to="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
     <remap from="gnss_pose_cov" to="$(var sub_gnss_pose_cov)"/>

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

@@ -20,6 +20,7 @@
 #include "ndt_localization_trigger_module.hpp"
 #include "ndt_module.hpp"
 #include "stop_check_module.hpp"
+#include "yabloc_module.hpp"
 
 #include <memory>
 #include <vector>
@@ -41,6 +42,9 @@
   if (declare_parameter<bool>("gnss_enabled")) {
     gnss_ = std::make_unique<GnssModule>(this);
   }
+  if (declare_parameter<bool>("yabloc_enabled")) {
+    yabloc_ = std::make_unique<YabLocModule>(this);
+  }
   if (declare_parameter<bool>("ndt_enabled")) {
     ndt_ = std::make_unique<NdtModule>(this);
     ndt_localization_trigger_ = std::make_unique<NdtLocalizationTriggerModule>(this);
@@ -86,6 +90,10 @@
     auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
     if (ndt_) {
       pose = ndt_->align_pose(pose);
+    } else if (yabloc_) {
+      // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
+      // accuracy pose.
+      pose = yabloc_->align_pose(pose);
     }
     pose.pose.covariance = output_pose_covariance_;
     pub_reset_->publish(pose);

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

@@ -26,6 +26,7 @@
 class StopCheckModule;
 class NdtModule;
 class GnssModule;
+class YabLocModule;
 class EkfLocalizationTriggerModule;
 class NdtLocalizationTriggerModule;
 
@@ -50,6 +51,7 @@ class PoseInitializer : public rclcpp::Node
   std::array<double, 36> gnss_particle_covariance_;
   std::unique_ptr<GnssModule> gnss_;
   std::unique_ptr<NdtModule> ndt_;
+  std::unique_ptr<YabLocModule> yabloc_;
   std::unique_ptr<StopCheckModule> stop_check_;
   std::unique_ptr<EkfLocalizationTriggerModule> ekf_localization_trigger_;
   std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;

localization/pose_initializer/src/pose_initializer/yabloc_module.cpp

@@ -0,0 +1,51 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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 "yabloc_module.hpp"
+
+#include <component_interface_specs/localization.hpp>
+#include <component_interface_utils/rclcpp/exceptions.hpp>
+
+#include <memory>
+
+using ServiceException = component_interface_utils::ServiceException;
+using Initialize = localization_interface::Initialize;
+using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger())
+{
+  cli_align_ = node->create_client<RequestPoseAlignment>("yabloc_align");
+}
+
+PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose)
+{
+  const auto req = std::make_shared<RequestPoseAlignment::Request>();
+  req->pose_with_covariance = pose;
+
+  if (!cli_align_->service_is_ready()) {
+    throw component_interface_utils::ServiceUnready("YabLoc align server is not ready.");
+  }
+
+  RCLCPP_INFO(logger_, "Call YabLoc align server.");
+  const auto res = cli_align_->async_send_request(req).get();
+  if (!res->success) {
+    RCLCPP_INFO(logger_, "YabLoc align server failed.");
+    throw ServiceException(
+      Initialize::Service::Response::ERROR_ESTIMATION, "YabLoc align server failed.");
+  }
+  RCLCPP_INFO(logger_, "YabLoc align server succeeded.");
+
+  // Overwrite the covariance.
+  return res->pose_with_covariance;
+}

localization/pose_initializer/src/pose_initializer/yabloc_module.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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.
+
+#ifndef POSE_INITIALIZER__YABLOC_MODULE_HPP_
+#define POSE_INITIALIZER__YABLOC_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+
+class YabLocModule
+{
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+public:
+  explicit YabLocModule(rclcpp::Node * node);
+  PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose);
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Client<RequestPoseAlignment>::SharedPtr cli_align_;
+};
+
+#endif  // POSE_INITIALIZER__YABLOC_MODULE_HPP_

localization/yabloc/README.md

@@ -0,0 +1,31 @@
+# YabLoc
+
+**YabLoc** is vision-based localization with vector map. [https://youtu.be/Eaf6r_BNFfk](https://youtu.be/Eaf6r_BNFfk)
+
+[![thumbnail](docs/yabloc_thumbnail.jpg)](https://youtu.be/Eaf6r_BNFfk)
+
+## Packages
+
+- [yabloc_common](yabloc_common/README.md)
+- [yabloc_image_processing](yabloc_image_processing/README.md)
+- [yabloc_particle_filter](yabloc_particle_filter/README.md)
+- [yabloc_pose_initializer](yabloc_pose_initializer/README.md)
+
+## How to launch YabLoc instead of NDT
+
+When launching autoware, if you set `localization_mode:=yabloc` as an argument, YabLoc will be launched instead of NDT.
+By default, `localization_mode` is `ndt`.
+
+A sample command to run YabLoc is as follows
+
+```shell
+ros2 launch autoware_launch logging_simulator.launch.xml \
+  map_path:=$HOME/autoware_map/sample-map-rosbag\
+  vehicle_model:=sample_vehicle \
+  sensor_model:=sample_sensor_kit \
+  localization_mode:=yabloc
+```
+
+## Architecture
+
+![node_diagram](docs/yabloc_architecture.drawio.svg)