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..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()), default_max_velocity_(default_max_velocity) { // 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); } }