feat(pose_initilizer): set intial pose directly (#6692)

YamatoAndo · pre-commit-ci[bot] · web-flow · commit 8c02272f86b3 · 2024-04-01T15:50:23.000+09:00
* feat(pose_initilizer): set intial pose directly

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* style(pre-commit): autofix

* fix arg order

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* minor change

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* style(pre-commit): autofix

* remove blank lines

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* change types

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* add wait_for_service

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* style(pre-commit): autofix

* fix default quaternion

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* rename params

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* input quaternion validation

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* fix message

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* style(pre-commit): autofix

* add std::abs

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;

* style(pre-commit): autofix

---------

Signed-off-by: Yamato Ando &lt;yamato.ando@tier4.jp&gt;
Co-authored-by: pre-commit-ci[bot] &lt;66853113+pre-commit-ci[bot]@users.noreply.github.com&gt;
diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -2,6 +2,7 @@
 <launch>
   <arg name="pose_source" description="A string consisting of ndt, yabloc, artag and eagleye joined by underscores no matter the order. e.g. ndt_yabloc"/>
   <arg name="twist_source"/>
+  <arg name="initial_pose"/>
   <arg name="system_run_mode"/>
 
   <!-- parameter paths for ndt -->
diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
@@ -21,6 +21,11 @@
   <let name="use_eagleye_twist" value="$(eval &quot;'eagleye' == '$(var twist_source)'&quot;)"/>
   <let name="use_gyro_odom_twist" value="$(eval &quot;'gyro_odom' == '$(var twist_source)'&quot;)"/>
 
+  <!-- set initial pose -->
+  <let name="user_defined_initial_pose/enable" value="$(eval &quot;len($(var initial_pose)) == 7&quot;)"/>
+  <let name="user_defined_initial_pose/pose" value="$(var initial_pose)" if="$(var user_defined_initial_pose/enable)"/>
+  <let name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]" unless="$(var user_defined_initial_pose/enable)"/>
+
   <!-- NDT Scan Matcher Launch (as pose estimator) -->
   <group if="$(var use_ndt_pose)">
     <push-ros-namespace namespace="pose_estimator"/>
@@ -91,6 +96,8 @@
     </group>
 
     <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="user_defined_initial_pose/enable" value="$(var user_defined_initial_pose/enable)"/>
+      <arg name="user_defined_initial_pose/pose" value="$(var user_defined_initial_pose/pose)"/>
       <arg name="ndt_enabled" value="$(var use_ndt_pose)"/>
       <arg name="yabloc_enabled" value="$(var use_yabloc_pose)"/>
       <arg name="gnss_enabled" value="$(var gnss_enabled)"/>
diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml
@@ -1,5 +1,9 @@
 /**:
   ros__parameters:
+    user_defined_initial_pose:
+      enable: $(var user_defined_initial_pose/enable)
+      pose: $(var user_defined_initial_pose/pose)
+
     gnss_pose_timeout: 3.0 # [sec]
     stop_check_duration: 3.0 # [sec]
     ekf_enabled: $(var ekf_enabled)
diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json
@@ -6,6 +6,23 @@
     "pose_initializer": {
       "type": "object",
       "properties": {
+        "user_defined_initial_pose": {
+          "type": "object",
+          "properties": {
+            "enable": {
+              "type": "string",
+              "description": "If true, user_defined_initial_pose.pose is set as the initial position. [boolean]",
+              "default": "false"
+            },
+            "pose": {
+              "type": "string",
+              "description": "initial pose (x, y, z, quat_x, quat_y, quat_z, quat_w). [array]",
+              "default": "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"
+            }
+          },
+          "required": ["enable", "pose"],
+          "additionalProperties": false
+        },
         "gnss_pose_timeout": {
           "type": "number",
           "description": "The duration that the GNSS pose is valid. [sec]",
@@ -55,6 +72,7 @@
         }
       },
       "required": [
+        "user_defined_initial_pose",
         "gnss_pose_timeout",
         "stop_check_duration",
         "ekf_enabled",
diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp
@@ -23,13 +23,20 @@
 using ServiceException = component_interface_utils::ServiceException;
 using Initialize = localization_interface::Initialize;
 
-EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node)
-: logger_(node->get_logger())
+EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) : node_(node)
 {
-  client_ekf_trigger_ = node->create_client<SetBool>("ekf_trigger_node");
+  client_ekf_trigger_ = node_->create_client<SetBool>("ekf_trigger_node");
 }
 
-void EkfLocalizationTriggerModule::send_request(bool flag) const
+void EkfLocalizationTriggerModule::wait_for_service()
+{
+  while (!client_ekf_trigger_->wait_for_service(std::chrono::seconds(1))) {
+    RCLCPP_INFO(node_->get_logger(), "EKF triggering service is not available, waiting...");
+  }
+  RCLCPP_INFO(node_->get_logger(), "EKF triggering service is available!");
+}
+
+void EkfLocalizationTriggerModule::send_request(bool flag, bool need_spin) const
 {
   const auto req = std::make_shared<SetBool::Request>();
   std::string command_name;
@@ -46,10 +53,14 @@ void EkfLocalizationTriggerModule::send_request(bool flag) const
 
   auto future_ekf = client_ekf_trigger_->async_send_request(req);
 
+  if (need_spin) {
+    rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ekf);
+  }
+
   if (future_ekf.get()->success) {
-    RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "EKF %s succeeded", command_name.c_str());
   } else {
-    RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "EKF %s failed", command_name.c_str());
     throw ServiceException(
       Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed");
   }
diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp
@@ -26,10 +26,11 @@ class EkfLocalizationTriggerModule
 
 public:
   explicit EkfLocalizationTriggerModule(rclcpp::Node * node);
-  void send_request(bool flag) const;
+  void wait_for_service();
+  void send_request(bool flag, bool need_spin = false) const;
 
 private:
-  rclcpp::Logger logger_;
+  rclcpp::Node * node_;
   rclcpp::Client<SetBool>::SharedPtr client_ekf_trigger_;
 };
 
diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp
@@ -23,13 +23,20 @@
 using ServiceException = component_interface_utils::ServiceException;
 using Initialize = localization_interface::Initialize;
 
-NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node)
-: logger_(node->get_logger())
+NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) : node_(node)
 {
-  client_ndt_trigger_ = node->create_client<SetBool>("ndt_trigger_node");
+  client_ndt_trigger_ = node_->create_client<SetBool>("ndt_trigger_node");
 }
 
-void NdtLocalizationTriggerModule::send_request(bool flag) const
+void NdtLocalizationTriggerModule::wait_for_service()
+{
+  while (!client_ndt_trigger_->wait_for_service(std::chrono::seconds(1))) {
+    RCLCPP_INFO(node_->get_logger(), "NDT triggering service is not available, waiting...");
+  }
+  RCLCPP_INFO(node_->get_logger(), "NDT triggering service is available!");
+}
+
+void NdtLocalizationTriggerModule::send_request(bool flag, bool need_spin) const
 {
   const auto req = std::make_shared<SetBool::Request>();
   std::string command_name;
@@ -46,10 +53,14 @@ void NdtLocalizationTriggerModule::send_request(bool flag) const
 
   auto future_ndt = client_ndt_trigger_->async_send_request(req);
 
+  if (need_spin) {
+    rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ndt);
+  }
+
   if (future_ndt.get()->success) {
-    RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "NDT %s succeeded", command_name.c_str());
   } else {
-    RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "NDT %s failed", command_name.c_str());
     throw ServiceException(
       Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed");
   }
diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp
@@ -26,10 +26,11 @@ class NdtLocalizationTriggerModule
 
 public:
   explicit NdtLocalizationTriggerModule(rclcpp::Node * node);
-  void send_request(bool flag) const;
+  void wait_for_service();
+  void send_request(bool flag, bool need_spin = false) const;
 
 private:
-  rclcpp::Logger logger_;
+  rclcpp::Node * node_;
   rclcpp::Client<SetBool>::SharedPtr client_ndt_trigger_;
 };
 
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -57,6 +57,31 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 
   change_state(State::Message::UNINITIALIZED);
+
+  if (declare_parameter<bool>("user_defined_initial_pose.enable")) {
+    const auto initial_pose_array =
+      declare_parameter<std::vector<double>>("user_defined_initial_pose.pose");
+    if (initial_pose_array.size() != 7) {
+      throw std::invalid_argument(
+        "Could not set user defined initial pose. The size of initial_pose is " +
+        std::to_string(initial_pose_array.size()) + ". It must be 7.");
+    } else if (
+      std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 &&
+      std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) {
+      throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero.");
+    } else {
+      geometry_msgs::msg::Pose initial_pose;
+      initial_pose.position.x = initial_pose_array[0];
+      initial_pose.position.y = initial_pose_array[1];
+      initial_pose.position.z = initial_pose_array[2];
+      initial_pose.orientation.x = initial_pose_array[3];
+      initial_pose.orientation.y = initial_pose_array[4];
+      initial_pose.orientation.z = initial_pose_array[5];
+      initial_pose.orientation.w = initial_pose_array[6];
+
+      set_user_defined_initial_pose(initial_pose);
+    }
+  }
 }
 
 PoseInitializer::~PoseInitializer()
@@ -71,6 +96,47 @@ void PoseInitializer::change_state(State::Message::_state_type state)
   pub_state_->publish(state_);
 }
 
+// To execute in the constructor, you need to call ros spin.
+// Conversely, ros spin should not be called elsewhere
+void PoseInitializer::change_node_trigger(bool flag, bool need_spin)
+{
+  try {
+    if (ekf_localization_trigger_) {
+      ekf_localization_trigger_->wait_for_service();
+      ekf_localization_trigger_->send_request(flag, need_spin);
+    }
+    if (ndt_localization_trigger_) {
+      ndt_localization_trigger_->wait_for_service();
+      ndt_localization_trigger_->send_request(flag, need_spin);
+    }
+  } catch (const ServiceException & error) {
+    throw;
+  }
+}
+
+void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose)
+{
+  try {
+    change_state(State::Message::INITIALIZING);
+    change_node_trigger(false, true);
+
+    PoseWithCovarianceStamped pose;
+    pose.header.frame_id = "map";
+    pose.header.stamp = now();
+    pose.pose.pose = initial_pose;
+    pose.pose.covariance = output_pose_covariance_;
+    pub_reset_->publish(pose);
+
+    change_node_trigger(true, true);
+    change_state(State::Message::INITIALIZED);
+
+    RCLCPP_INFO(get_logger(), "Set user defined initial pose");
+  } catch (const ServiceException & error) {
+    change_state(State::Message::UNINITIALIZED);
+    RCLCPP_WARN(get_logger(), "Could not set user defined initial pose");
+  }
+}
+
 void PoseInitializer::on_initialize(
   const Initialize::Service::Request::SharedPtr req,
   const Initialize::Service::Response::SharedPtr res)
@@ -82,12 +148,8 @@ void PoseInitializer::on_initialize(
   }
   try {
     change_state(State::Message::INITIALIZING);
-    if (ekf_localization_trigger_) {
-      ekf_localization_trigger_->send_request(false);
-    }
-    if (ndt_localization_trigger_) {
-      ndt_localization_trigger_->send_request(false);
-    }
+    change_node_trigger(false, false);
+
     auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
     if (ndt_) {
       pose = ndt_->align_pose(pose);
@@ -98,12 +160,8 @@ void PoseInitializer::on_initialize(
     }
     pose.pose.covariance = output_pose_covariance_;
     pub_reset_->publish(pose);
-    if (ekf_localization_trigger_) {
-      ekf_localization_trigger_->send_request(true);
-    }
-    if (ndt_localization_trigger_) {
-      ndt_localization_trigger_->send_request(true);
-    }
+
+    change_node_trigger(true, false);
     res->status.success = true;
     change_state(State::Message::INITIALIZED);
   } catch (const ServiceException & error) {
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp
@@ -58,6 +58,9 @@ class PoseInitializer : public rclcpp::Node
   std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
   double stop_check_duration_;
+
+  void change_node_trigger(bool flag, bool need_spin = false);
+  void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose);
   void change_state(State::Message::_state_type state);
   void on_initialize(
     const Initialize::Service::Request::SharedPtr req,
