From 02db29b9195b3e71cb6e3abefecd951987bc02ff Mon Sep 17 00:00:00 2001
From: uhobeike <beike315@icloud.com>
Date: Thu, 16 Nov 2023 15:04:27 +0900
Subject: [PATCH 1/2] Comment out std::cerr

---
 ike_localization/src/mcl/motionModel.cpp      |  8 ++++----
 ike_localization/src/mcl/observationModel.cpp | 16 ++++++++--------
 ike_localization/src/mcl/resampling.cpp       |  8 ++++----
 3 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/ike_localization/src/mcl/motionModel.cpp b/ike_localization/src/mcl/motionModel.cpp
index c39dcf5..c68e411 100644
--- a/ike_localization/src/mcl/motionModel.cpp
+++ b/ike_localization/src/mcl/motionModel.cpp
@@ -27,8 +27,8 @@ void MotionModel::update(
   std::vector<Particle> & particles, double current_pose_yaw, double delta_x, double delta_y,
   double delta_yaw)
 {
-  std::cerr << "Run MotionModel::update."
-            << "\n";
+  // std::cerr << "Run MotionModel::update."
+  //           << "\n";
 
   double delta_rotate_1, delta_rotate_2, delta_trans;
   double delta_rotate_1_hat, delta_rotate_2_hat, delta_trans_hat;
@@ -68,8 +68,8 @@ void MotionModel::update(
     p.pose.euler.yaw = normalizeAngle(p.pose.euler.yaw) + delta_rotate_1_hat + delta_rotate_2_hat;
   }
 
-  std::cerr << "Done MotionModel::update."
-            << "\n";
+  // std::cerr << "Done MotionModel::update."
+  //           << "\n";
 }
 
 double MotionModel::drawNoise(double sigma)
diff --git a/ike_localization/src/mcl/observationModel.cpp b/ike_localization/src/mcl/observationModel.cpp
index b8889fa..2bb4b8e 100644
--- a/ike_localization/src/mcl/observationModel.cpp
+++ b/ike_localization/src/mcl/observationModel.cpp
@@ -40,20 +40,20 @@ void ObservationModel::initScan(
 
 void ObservationModel::setScan(std::vector<float> & scan_data)
 {
-  std::cerr << "Run ObservationModel::setScan."
-            << "\n";
+  // std::cerr << "Run ObservationModel::setScan."
+  //           << "\n";
 
   scan_.ranges.resize(scan_data.size());
   std::copy(std::begin(scan_data), std::end(scan_data), std::begin(scan_.ranges));
 
-  std::cerr << "Done ObservationModel::setScan."
-            << "\n";
+  // std::cerr << "Done ObservationModel::setScan."
+  //           << "\n";
 }
 
 void ObservationModel::update(std::vector<Particle> & particles, std::vector<float> scan_data)
 {
-  std::cerr << "Run ObservationModel::update."
-            << "\n";
+  // std::cerr << "Run ObservationModel::update."
+  //           << "\n";
 
   setScan(scan_data);
 
@@ -67,8 +67,8 @@ void ObservationModel::update(std::vector<Particle> & particles, std::vector<flo
 
   marginal_likelihood_ = sum_score / (particles.size() * scan_.ranges.size());
 
-  std::cerr << "Done ObservationModel::update."
-            << "\n";
+  // std::cerr << "Done ObservationModel::update."
+  //           << "\n";
 }
 
 double ObservationModel::calculateParticleWeight(const Particle p)
diff --git a/ike_localization/src/mcl/resampling.cpp b/ike_localization/src/mcl/resampling.cpp
index c36865c..bcac459 100644
--- a/ike_localization/src/mcl/resampling.cpp
+++ b/ike_localization/src/mcl/resampling.cpp
@@ -13,14 +13,14 @@ Resampling::~Resampling(){};
 
 void Resampling::resampling(std::vector<Particle> & particles)
 {
-  std::cerr << "Run Resampling::resampling."
-            << "\n";
+  // std::cerr << "Run Resampling::resampling."
+  //           << "\n";
 
   normalize(particles);
   systematicSampling(particles);
 
-  std::cerr << "Done Resampling::resampling."
-            << "\n";
+  // std::cerr << "Done Resampling::resampling."
+  //           << "\n";
 }
 
 // 詳解 確率ロボティクス p.131

From 50589069f0e52b63af49ac90d4c19a4f348444b6 Mon Sep 17 00:00:00 2001
From: uhobeike <beike315@icloud.com>
Date: Thu, 16 Nov 2023 15:04:50 +0900
Subject: [PATCH 2/2] Fix suppress excessive standard output from canTransform

---
 ike_localization/src/ike_localization.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/ike_localization/src/ike_localization.cpp b/ike_localization/src/ike_localization.cpp
index d130eb3..b0fb32e 100644
--- a/ike_localization/src/ike_localization.cpp
+++ b/ike_localization/src/ike_localization.cpp
@@ -243,6 +243,7 @@ void IkeLocalization::getCurrentRobotPose(geometry_msgs::msg::PoseStamped & curr
   while (rclcpp::ok() &&
          not tf_buffer_->canTransform(odom_frame_, robot_frame_, tf2::TimePoint())) {
     RCLCPP_WARN(get_logger(), "Wait Can Transform");
+    rclcpp::sleep_for(std::chrono::seconds(1));
   }
 
   geometry_msgs::msg::PoseStamped robot_pose;