Pose initialization keeps running via automatic_pose_initializer

[kminoda]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

automatic_pose_initializer, which was recently added to autoware.universe in #1431, keeps running the pose initialization request under certain condition. The condition is that the timer_ period is set lower than the time it takes to calculate the pose initialization.

  const auto period = rclcpp::Rate(1.0).period();
  timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); });

The default value rclcpp::Rate(1.0) works fine on my PC, but for example by setting this value to rclcpp::Rate(3.0), the pose initialization request keep running.

infinite_pose_initialization-2022-09-01_10.26.56.mp4

Expected behavior

The automatic_pose_initializer should run only one single successful pose initialization request.

Actual behavior

As mentioned above, the package keeps running the initialization when the rate is set high enough.

Steps to reproduce

As mentioned above, in l.29 from system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp

  const auto period = rclcpp::Rate(1.0).period();

change the value 1.0 to some higher value, e.g. 3.0.

Versions

• OS: Ubuntu 22.04
• ROS 2: Humble
• Autoware: dcf99e5

Possible causes

It is possible that the sub_state_ is blocked by timer_ and cannot update the state_.state from UNINITIALIZED to INITIALIZING or INITIALIZED.

Additional context

No response

[kminoda]

How about this? @isamu-takagi @YamatoAndo

void AutomaticPoseInitializer::on_timer()
{
  if (state_.state == State::Message::UNINITIALIZED) {
    try {
      const auto req = std::make_shared<Initialize::Service::Request>();
      cli_initialize_->call(req);
    } catch (const component_interface_utils::ServiceException & error) {
    }
  }
}

to

void AutomaticPoseInitializer::on_timer()
{
  if (state_.state == State::Message::UNINITIALIZED) {
    try {
      const auto req = std::make_shared<Initialize::Service::Request>();
      cli_initialize_->call(req);
      const auto res = cli_initialize_->async_send_request(req).get();
      if (!res->status.success) {
        state_.state = State::Message::UNINITIALIZED;
      } else {
        state_.state = State::Message::INITIALIZED;
      }
    } catch (const component_interface_utils::ServiceException & error) {
    }
  }
}

[isamu-takagi]

Thank you for reporting. I will check it.

[isamu-takagi]

Fixed in this PR. #1756