Skip to content

Commit 6744b1a

Browse files
fix(ndt_scan_matcher): changed the type of timestamp from double to int in ndt diag (#7128)
Changed the type of timestamp from double to int in ndt diag Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent 84e33d8 commit 6744b1a

File tree

2 files changed

+7
-6
lines changed

2 files changed

+7
-6
lines changed

localization/ndt_scan_matcher/src/map_update_module.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ void MapUpdateModule::callback_timer(
5252
const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position,
5353
std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
5454
{
55-
diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds());
55+
diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().nanoseconds());
5656

5757
// check is_activated
5858
diagnostics_ptr->addKeyValue("is_activated", is_activated);

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+6-5
Original file line numberDiff line numberDiff line change
@@ -214,7 +214,8 @@ void NDTScanMatcher::callback_initial_pose_main(
214214
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr)
215215
{
216216
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());
218219

219220
// check is_activated
220221
diagnostics_initial_pose_->addKeyValue("is_activated", static_cast<bool>(is_activated_));
@@ -255,7 +256,7 @@ void NDTScanMatcher::callback_regularization_pose(
255256
diagnostics_regularization_pose_->clear();
256257

257258
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());
259260

260261
regularization_pose_buffer_->push_back(pose_conv_msg_ptr);
261262

@@ -295,7 +296,7 @@ bool NDTScanMatcher::callback_sensor_points_main(
295296

296297
// check topic_time_stamp
297298
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());
299300

300301
// check sensor_points_size
301302
const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width;
@@ -867,7 +868,7 @@ void NDTScanMatcher::service_trigger_node(
867868
std_srvs::srv::SetBool::Response::SharedPtr res)
868869
{
869870
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());
871872

872873
is_activated_ = req->data;
873874
if (is_activated_) {
@@ -905,7 +906,7 @@ void NDTScanMatcher::service_ndt_align_main(
905906
const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
906907
tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
907908
{
908-
diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds());
909+
diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().nanoseconds());
909910

910911
// get TF from pose_frame to map_frame
911912
const std::string & target_frame = param_.frame.map_frame;

0 commit comments

Comments
 (0)