diff --git a/include/clock_publisher/clock_publisher.hpp b/include/clock_publisher/clock_publisher.hpp index 5e6fe8b..3652082 100644 --- a/include/clock_publisher/clock_publisher.hpp +++ b/include/clock_publisher/clock_publisher.hpp @@ -13,7 +13,7 @@ // limitations under the License. #ifndef CLOCK_PUBLISHER_CLOCK_PUBLISHER_H -#define CLOCK_PUBLISHER_CLOCK_PUBLISHER_H +#define CLOCK_PUBLISHER_CLOCK_PUBLISHER_H #include @@ -33,6 +33,9 @@ class ClockPublisher : public rclcpp::Node void onTimer(); rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time base_time_; + double speed_rate_; + bool time_start_from_zero_; }; #endif diff --git a/launch/clock_publisher.launch.xml b/launch/clock_publisher.launch.xml index b26ba53..03ef1b3 100644 --- a/launch/clock_publisher.launch.xml +++ b/launch/clock_publisher.launch.xml @@ -1,6 +1,10 @@ + + + + diff --git a/src/clock_publisher.cpp b/src/clock_publisher.cpp index 1601edc..4877cf5 100644 --- a/src/clock_publisher.cpp +++ b/src/clock_publisher.cpp @@ -18,22 +18,31 @@ void ClockPublisher::onTimer() { + const rclcpp::Duration dif_time = (this->now() - base_time_) * speed_rate_; + rclcpp::Time current_time; + if (time_start_from_zero_) { + current_time = rclcpp::Time(0) + dif_time; + } else { + current_time = base_time_ + dif_time; + } rosgraph_msgs::msg::Clock clock; - clock.clock = this->now(); - // std::cerr << std::fixed << rclcpp::Time(clock.clock).seconds() << std::endl; - clock_pub_->publish(clock); + clock.set__clock(current_time); + clock_pub_->publish(clock); } ClockPublisher::ClockPublisher(const rclcpp::NodeOptions & node_options) : Node("clock_publisher", node_options) { + base_time_ = this->now(); clock_pub_ = this->create_publisher("/clock", 1); const int rate = this->declare_parameter("rate", 200); + speed_rate_ = this->declare_parameter("speed_rate", 1.0); + time_start_from_zero_ = this->declare_parameter("time_start_from_zero", false); // Timer auto timer_callback = std::bind(&ClockPublisher::onTimer, this); auto period = std::chrono::duration_cast( - std::chrono::duration(1.0/static_cast(rate))); + std::chrono::duration(1.0 / static_cast(rate))); timer_ = std::make_shared>( this->get_clock(), period, std::move(timer_callback),