@@ -214,7 +214,8 @@ void NDTScanMatcher::callback_initial_pose_main(
214
214
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr)
215
215
{
216
216
diagnostics_initial_pose_->addKeyValue (
217
- " topic_time_stamp" , static_cast <rclcpp::Time>(initial_pose_msg_ptr->header .stamp ).seconds ());
217
+ " topic_time_stamp" ,
218
+ static_cast <rclcpp::Time>(initial_pose_msg_ptr->header .stamp ).nanoseconds ());
218
219
219
220
// check is_activated
220
221
diagnostics_initial_pose_->addKeyValue (" is_activated" , static_cast <bool >(is_activated_));
@@ -255,7 +256,7 @@ void NDTScanMatcher::callback_regularization_pose(
255
256
diagnostics_regularization_pose_->clear ();
256
257
257
258
diagnostics_regularization_pose_->addKeyValue (
258
- " topic_time_stamp" , static_cast <rclcpp::Time>(pose_conv_msg_ptr->header .stamp ).seconds ());
259
+ " topic_time_stamp" , static_cast <rclcpp::Time>(pose_conv_msg_ptr->header .stamp ).nanoseconds ());
259
260
260
261
regularization_pose_buffer_->push_back (pose_conv_msg_ptr);
261
262
@@ -295,7 +296,7 @@ bool NDTScanMatcher::callback_sensor_points_main(
295
296
296
297
// check topic_time_stamp
297
298
const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header .stamp ;
298
- diagnostics_scan_points_->addKeyValue (" topic_time_stamp" , sensor_ros_time.seconds ());
299
+ diagnostics_scan_points_->addKeyValue (" topic_time_stamp" , sensor_ros_time.nanoseconds ());
299
300
300
301
// check sensor_points_size
301
302
const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width ;
@@ -867,7 +868,7 @@ void NDTScanMatcher::service_trigger_node(
867
868
std_srvs::srv::SetBool::Response::SharedPtr res)
868
869
{
869
870
diagnostics_trigger_node_->clear ();
870
- diagnostics_trigger_node_->addKeyValue (" service_call_time_stamp" , this ->now ().seconds ());
871
+ diagnostics_trigger_node_->addKeyValue (" service_call_time_stamp" , this ->now ().nanoseconds ());
871
872
872
873
is_activated_ = req->data ;
873
874
if (is_activated_) {
@@ -905,7 +906,7 @@ void NDTScanMatcher::service_ndt_align_main(
905
906
const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
906
907
tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
907
908
{
908
- diagnostics_ndt_align_->addKeyValue (" service_call_time_stamp" , this ->now ().seconds ());
909
+ diagnostics_ndt_align_->addKeyValue (" service_call_time_stamp" , this ->now ().nanoseconds ());
909
910
910
911
// get TF from pose_frame to map_frame
911
912
const std::string & target_frame = param_.frame .map_frame ;
0 commit comments