diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp index bc62bfa690946..201c743471efd 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -40,6 +40,7 @@ std::optional SmartPoseBuffer::interpolate( const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; if (target_ros_time < time_first) { + RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); return std::nullopt; } diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index bb32741067e65..ae6f0b61f063c 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -241,7 +241,7 @@ void output_pose_with_cov_to_log( rpy.y = rpy.y * 180.0 / M_PI; rpy.z = rpy.z * 180.0 / M_PI; - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 0132e6edf8667..286540a12ac72 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -182,7 +182,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); return true; // Updated } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index cd637791f04b6..711dd84e9ea03 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -992,7 +992,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); - RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score); return result_pose_with_cov_msg; } diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 801a08f83aab8..6e11f8fe74212 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -40,7 +40,6 @@ PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped RCLCPP_INFO(logger_, "Call NDT align server."); const auto res = cli_align_->async_send_request(req).get(); if (!res->success) { - RCLCPP_INFO(logger_, "NDT align server failed."); throw ServiceException( Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed."); } diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f6407c532b5f8..6a26014b3e014 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -145,10 +145,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Subscribers { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); for (auto & input_topic : input_topics_) { - RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); } // Subscribe to the filters diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 57a563c28a4a6..c4364ce1b06e3 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -77,7 +77,7 @@ pointcloud_preprocessor::Filter::Filter( latched_indices_ = static_cast(declare_parameter("latched_indices", false)); approximate_sync_ = static_cast(declare_parameter("approximate_sync", false)); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( this->get_logger(), "Filter (as Component) successfully created with the following parameters:" << std::endl