Fix suppress excessive standard output from canTransform (#83)

* Comment out std::cerr * Fix suppress excessive standard output from canTransform
uhobeike · Nov 16, 2023 · ffedddb · ffedddb
1 parent f076bfd
commit ffedddb
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 16 deletions.
diff --git 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;

diff --git 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
@@ -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
@@ -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