fix(pose_initializer): fix response of initalize pose API (#1020)

* fix(pose_initializer): fix response of initalize pose API

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* ci(pre-commit): autofix

* fix(pose_initializer): fix member variable name

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp

@@ -71,6 +71,7 @@ class PoseInitializer : public rclcpp::Node
 
   rclcpp::Client<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr ndt_client_;
 
+  rclcpp::CallbackGroup::SharedPtr initialize_pose_service_group_;
   rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr
     initialize_pose_service_;
   rclcpp::Service<tier4_external_api_msgs::srv::InitializePoseAuto>::SharedPtr

localization/pose_initializer/src/pose_initializer_core.cpp

@@ -88,8 +88,10 @@ PoseInitializer::PoseInitializer()
   initial_pose_pub_ =
     this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose3d", 10);
 
-  ndt_client_ =
-    this->create_client<tier4_localization_msgs::srv::PoseWithCovarianceStamped>("ndt_align_srv");
+  initialize_pose_service_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  ndt_client_ = this->create_client<tier4_localization_msgs::srv::PoseWithCovarianceStamped>(
+    "ndt_align_srv", rmw_qos_profile_services_default, initialize_pose_service_group_);
   while (!ndt_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
     RCLCPP_INFO(get_logger(), "Waiting for service...");
   }
@@ -221,24 +223,26 @@ bool PoseInitializer::callAlignServiceAndPublishResult(
   req->seq = ++request_id_;
 
   RCLCPP_INFO(get_logger(), "call NDT Align Server");
+  auto result = ndt_client_->async_send_request(req).get();
+
+  if (!result->success) {
+    RCLCPP_INFO(get_logger(), "failed NDT Align Server");
+    response_id_ = result->seq;
+    return false;
+  }
+
+  RCLCPP_INFO(get_logger(), "called NDT Align Server");
+  response_id_ = result->seq;
+  // NOTE temporary cov
+  geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov = result->pose_with_covariance;
+  pose_with_cov.pose.covariance[0] = 1.0;
+  pose_with_cov.pose.covariance[1 * 6 + 1] = 1.0;
+  pose_with_cov.pose.covariance[2 * 6 + 2] = 0.01;
+  pose_with_cov.pose.covariance[3 * 6 + 3] = 0.01;
+  pose_with_cov.pose.covariance[4 * 6 + 4] = 0.01;
+  pose_with_cov.pose.covariance[5 * 6 + 5] = 0.2;
+  initial_pose_pub_->publish(pose_with_cov);
+  enable_gnss_callback_ = false;
 
-  ndt_client_->async_send_request(
-    req,
-    [this](rclcpp::Client<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture
-             result) {
-      if (result.get()->success) {
-        RCLCPP_INFO(get_logger(), "called NDT Align Server");
-        response_id_ = result.get()->seq;
-        // NOTE temporary cov
-        geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_covariance =
-          result.get()->pose_with_covariance;
-        pose_with_covariance.pose.covariance = output_pose_covariance_;
-        initial_pose_pub_->publish(pose_with_covariance);
-        enable_gnss_callback_ = false;
-      } else {
-        RCLCPP_INFO(get_logger(), "failed NDT Align Server");
-        response_id_ = result.get()->seq;
-      }
-    });
   return true;
 }

localization/pose_initializer/src/pose_initializer_node.cpp

@@ -21,7 +21,11 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<PoseInitializer>());
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<PoseInitializer>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
   rclcpp::shutdown();
   return 0;
 }