retry gnss initialize (autowarefoundation#1043) (autowarefoundation#263)

Co-authored-by: YamatoAndo <yamato.ando@gmail.com>
kosuke55 · Jan 30, 2021 · e28b396 · e28b396
1 parent 6fc711e
commit e28b396
Showing 1 changed file with 1 addition and 2 deletions.
diff --git a/localization/util/pose_initializer/src/pose_initializer_core.cpp b/localization/util/pose_initializer/src/pose_initializer_core.cpp
@@ -128,8 +128,6 @@ void PoseInitializer::callbackInitialPose(
 void PoseInitializer::callbackGNSSPoseCov(
   geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr)
 {
-  gnss_pose_sub_ = nullptr;  // get only first topic
-
   // TODO(YamatoAndo) check service is available
 
   auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
@@ -206,5 +204,6 @@ void PoseInitializer::callAlignServiceAndPublishResult(
       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);
+      gnss_pose_sub_ = nullptr;
     });
 }