Merge pull request autowarefoundation#3 from SKT-r/feat/add_nerf_init…

…ializer

feat: add initializer to nerf

launch/tier4_localization_launch/launch/pose_twist_estimator/nerf_based_localizer.launch.xml

@@ -9,6 +9,7 @@
         <arg name="output_image" value="/localization/pose_estimator/nerf_image"/>
         <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
         <arg name="param_file" value="$(find-pkg-share nerf_based_localizer)/config/nerf_based_localizer.param.yaml"/>
+        <arg name="service_optimize_pose" value="/localization/pose_estimator/nerf_align_srv"/>
       </include>
     </group>
   </group>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -123,6 +123,7 @@
       <arg name="yabloc_enabled" value="$(var use_yabloc_pose)"/>
       <arg name="gnss_enabled" value="$(var gnss_enabled)"/>
       <arg name="ekf_enabled" value="true"/>
+      <arg name="nerf_enabled" value="$(var use_nerf_pose)"/>
       <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
       <arg name="config_file" value="$(var pose_initializer_param_path)"/>
       <arg name="sub_gnss_pose_cov" value="$(var sub_gnss_pose_cov)"/>

localization/nerf_based_localizer/src/nerf_based_localizer.cpp

@@ -139,6 +139,7 @@ void NerfBasedLocalizer::callback_initial_pose(
 
 void NerfBasedLocalizer::callback_image(const sensor_msgs::msg::Image::ConstSharedPtr image_msg_ptr)
 {
+  target_frame_ = image_msg_ptr->header.frame_id;
   image_msg_ptr_array_.push_back(image_msg_ptr);
   if (image_msg_ptr_array_.size() > 1) {
     image_msg_ptr_array_.pop_front();
@@ -158,8 +159,6 @@ void NerfBasedLocalizer::callback_image(const sensor_msgs::msg::Image::ConstShar
     initial_pose_msg_ptr_array_.back();
   initial_pose_msg_ptr_array_.pop_back();
 
-  target_frame_ = image_msg_ptr->header.frame_id;
-
   // Process
   const auto [pose_msg, image_msg, score_msg] = localize(pose_base_link->pose.pose, *image_msg_ptr);
 

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/nerf_module.cpp
   src/pose_initializer/yabloc_module.cpp
   src/pose_initializer/stop_check_module.cpp
   src/pose_initializer/ekf_localization_trigger_module.cpp

localization/pose_initializer/config/pose_initializer.param.yaml

@@ -10,6 +10,7 @@
     gnss_enabled: $(var gnss_enabled)
     yabloc_enabled: $(var yabloc_enabled)
     ndt_enabled: $(var ndt_enabled)
+    nerf_enabled: $(var nerf_enabled)
     stop_check_enabled: $(var stop_check_enabled)
 
     # from gnss

localization/pose_initializer/launch/pose_initializer.launch.xml

@@ -13,6 +13,7 @@
     <param from="$(var config_file)" allow_substs="true"/>
     <remap from="yabloc_align" to="/localization/pose_estimator/yabloc/initializer/yabloc_align_srv"/>
     <remap from="ndt_align" to="/localization/pose_estimator/ndt_align_srv"/>
+    <remap from="nerf_align" to="/localization/pose_estimator/nerf_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)"/>
     <remap from="pose_reset" to="/initialpose3d"/>

localization/pose_initializer/src/pose_initializer/nerf_module.cpp

@@ -0,0 +1,50 @@
+// 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 "nerf_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;
+
+NeRFModule::NeRFModule(rclcpp::Node * node) : logger_(node->get_logger())
+{
+  cli_align_ = node->create_client<RequestPoseAlignment>("nerf_align");
+}
+
+PoseWithCovarianceStamped NeRFModule::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("NeRF align server is not ready.");
+  }
+
+  RCLCPP_INFO(logger_, "Call NeRF align server.");
+  const auto res = cli_align_->async_send_request(req).get();
+  if (!res->success) {
+    throw ServiceException(
+      Initialize::Service::Response::ERROR_ESTIMATION, "NeRF align server failed.");
+  }
+  RCLCPP_INFO(logger_, "NeRF align server succeeded.");
+
+  // Overwrite the covariance.
+  return res->pose_with_covariance;
+}

localization/pose_initializer/src/pose_initializer/nerf_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__NERF_MODULE_HPP_
+#define POSE_INITIALIZER__NERF_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 NeRFModule
+{
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+public:
+  explicit NeRFModule(rclcpp::Node * node);
+  PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose);
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Client<RequestPoseAlignment>::SharedPtr cli_align_;
+};
+
+#endif  // POSE_INITIALIZER__NERF_MODULE_HPP_

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

@@ -19,6 +19,7 @@
 #include "gnss_module.hpp"
 #include "ndt_localization_trigger_module.hpp"
 #include "ndt_module.hpp"
+#include "nerf_module.hpp"
 #include "stop_check_module.hpp"
 #include "yabloc_module.hpp"
 
@@ -49,6 +50,9 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
     ndt_ = std::make_unique<NdtModule>(this);
     ndt_localization_trigger_ = std::make_unique<NdtLocalizationTriggerModule>(this);
   }
+  if (declare_parameter<bool>("nerf_enabled", false)) {
+    nerf_ = std::make_unique<NeRFModule>(this);
+  }
   if (declare_parameter<bool>("stop_check_enabled")) {
     // Add 1.0 sec margin for twist buffer.
     stop_check_duration_ = declare_parameter<double>("stop_check_duration");
@@ -157,6 +161,8 @@ void PoseInitializer::on_initialize(
       // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
       // accuracy pose.
       pose = yabloc_->align_pose(pose);
+    } else if (nerf_) {
+      pose = nerf_->align_pose(pose);
     }
     pose.pose.covariance = output_pose_covariance_;
     pub_reset_->publish(pose);

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

@@ -30,6 +30,7 @@ class GnssModule;
 class YabLocModule;
 class EkfLocalizationTriggerModule;
 class NdtLocalizationTriggerModule;
+class NeRFModule;
 
 class PoseInitializer : public rclcpp::Node
 {
@@ -53,6 +54,7 @@ class PoseInitializer : public rclcpp::Node
   std::unique_ptr<GnssModule> gnss_;
   std::unique_ptr<NdtModule> ndt_;
   std::unique_ptr<YabLocModule> yabloc_;
+  std::unique_ptr<NeRFModule> nerf_;
   std::unique_ptr<StopCheckModule> stop_check_;
   std::unique_ptr<EkfLocalizationTriggerModule> ekf_localization_trigger_;
   std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;