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

)

* feat(pose_initilizer): set intial pose directly

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* fix arg order

Signed-off-by: Yamato Ando <[email protected]>

* minor change

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* remove blank lines

Signed-off-by: Yamato Ando <[email protected]>

* change types

Signed-off-by: Yamato Ando <[email protected]>

* add wait_for_service

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* fix default quaternion

Signed-off-by: Yamato Ando <[email protected]>

* rename params

Signed-off-by: Yamato Ando <[email protected]>

* input quaternion validation

Signed-off-by: Yamato Ando <[email protected]>

* fix message

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* add std::abs

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

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)"/>

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)

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",

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");
  }

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_;
 };
 

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");
  }

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_;
 };
 

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

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,