diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 43174a39b797e..ae68dfc44c2c9 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,7 +52,7 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds()); + diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().nanoseconds()); // check is_activated diagnostics_ptr->addKeyValue("is_activated", is_activated); 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 624eb89671a41..4da1c6865dbb7 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -214,7 +214,8 @@ void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { diagnostics_initial_pose_->addKeyValue( - "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", + static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); @@ -255,7 +256,7 @@ void NDTScanMatcher::callback_regularization_pose( diagnostics_regularization_pose_->clear(); diagnostics_regularization_pose_->addKeyValue( - "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); @@ -295,7 +296,7 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.seconds()); + diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; @@ -867,7 +868,7 @@ void NDTScanMatcher::service_trigger_node( std_srvs::srv::SetBool::Response::SharedPtr res) { diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -905,7 +906,7 @@ void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame;