From 59170e32173aab46325587b5dda6bc22e9817d78 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 15:57:25 +0900 Subject: [PATCH 1/2] set timestamp to max_velocity topic Signed-off-by: Autumn60 --- .../awapi_awiv_adapter/awapi_max_velocity_publisher.hpp | 2 ++ awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp index 0e00c6c..04754a5 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -30,6 +30,8 @@ class AutowareIvMaxVelocityPublisher void statePublisher(const AutowareInfo & aw_info); private: + rclcpp::Clock::SharedPtr clock_; + // publisher rclcpp::Publisher::SharedPtr pub_state_; diff --git a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp index e68b5ef..ff17828 100644 --- a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp +++ b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -18,7 +18,7 @@ namespace autoware_api { AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( rclcpp::Node & node, const double default_max_velocity) -: default_max_velocity_(default_max_velocity) +: default_max_velocity_(default_max_velocity), clock_(node.get_clock()) { // publisher pub_state_ = node.create_publisher( @@ -32,6 +32,7 @@ void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, &max_velocity.max_velocity)) // publish info { + max_velocity.stamp = clock_->now(); pub_state_->publish(max_velocity); } } From 1d2c39cf5834d4324205148b33f3770d525a2309 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 16:04:11 +0900 Subject: [PATCH 2/2] fix constructor order Signed-off-by: Autumn60 --- awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp index ff17828..90f1e71 100644 --- a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp +++ b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -18,7 +18,7 @@ namespace autoware_api { AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( rclcpp::Node & node, const double default_max_velocity) -: default_max_velocity_(default_max_velocity), clock_(node.get_clock()) +: clock_(node.get_clock()), default_max_velocity_(default_max_velocity) { // publisher pub_state_ = node.create_publisher(