From e26dae7e600750e307bd731965de328ce96ee372 Mon Sep 17 00:00:00 2001
From: "tomoya.kimura" <tomoya.kimura@tier4.jp>
Date: Fri, 12 Mar 2021 18:14:42 +0900
Subject: [PATCH 1/2] add time rate

---
 include/clock_publisher/clock_publisher.hpp |  4 +++-
 launch/clock_publisher.launch.xml           |  2 ++
 src/clock_publisher.cpp                     | 11 +++++++----
 3 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/include/clock_publisher/clock_publisher.hpp b/include/clock_publisher/clock_publisher.hpp
index 5e6fe8b..f5c74cf 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 <string>
 
@@ -33,6 +33,8 @@ class ClockPublisher : public rclcpp::Node
   void onTimer();
   rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Time base_time_;
+  double speed_rate_;
 };
 
 #endif
diff --git a/launch/clock_publisher.launch.xml b/launch/clock_publisher.launch.xml
index b26ba53..3934926 100644
--- a/launch/clock_publisher.launch.xml
+++ b/launch/clock_publisher.launch.xml
@@ -1,6 +1,8 @@
 <launch>
   <arg name="rate" default="200"/>
+  <arg name="speed_rate" default="1.0"/>
   <node pkg="clock_publisher" exec="clock_publisher_node" name="clock_publisher" output="screen">
     <param name="rate" value="$(var rate)"/>
+    <param name="speed_rate" value="$(var speed_rate)"/>
   </node>
 </launch>
diff --git a/src/clock_publisher.cpp b/src/clock_publisher.cpp
index 1601edc..d3ab57d 100644
--- a/src/clock_publisher.cpp
+++ b/src/clock_publisher.cpp
@@ -18,22 +18,25 @@
 
 void ClockPublisher::onTimer()
 {
+  const rclcpp::Duration dif_time = (this->now() - base_time_) * speed_rate_;
+  const rclcpp::Time 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<rosgraph_msgs::msg::Clock>("/clock", 1);
   const int rate = this->declare_parameter("rate", 200);
+  speed_rate_ = this->declare_parameter("speed_rate", 1.0);
 
   // Timer
   auto timer_callback = std::bind(&ClockPublisher::onTimer, this);
   auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
-    std::chrono::duration<double>(1.0/static_cast<double>(rate)));
+    std::chrono::duration<double>(1.0 / static_cast<double>(rate)));
 
   timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
     this->get_clock(), period, std::move(timer_callback),

From 84fed77fc864dd63c3fcb9b6887d8e61bb57445c Mon Sep 17 00:00:00 2001
From: "tomoya.kimura" <tomoya.kimura@tier4.jp>
Date: Fri, 12 Mar 2021 18:25:22 +0900
Subject: [PATCH 2/2] add base_time option

---
 include/clock_publisher/clock_publisher.hpp |  1 +
 launch/clock_publisher.launch.xml           |  2 ++
 src/clock_publisher.cpp                     | 10 ++++++++--
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/include/clock_publisher/clock_publisher.hpp b/include/clock_publisher/clock_publisher.hpp
index f5c74cf..3652082 100644
--- a/include/clock_publisher/clock_publisher.hpp
+++ b/include/clock_publisher/clock_publisher.hpp
@@ -35,6 +35,7 @@ class ClockPublisher : public rclcpp::Node
   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 3934926..03ef1b3 100644
--- a/launch/clock_publisher.launch.xml
+++ b/launch/clock_publisher.launch.xml
@@ -1,8 +1,10 @@
 <launch>
   <arg name="rate" default="200"/>
   <arg name="speed_rate" default="1.0"/>
+  <arg name="time_start_from_zero" default="false"/>
   <node pkg="clock_publisher" exec="clock_publisher_node" name="clock_publisher" output="screen">
     <param name="rate" value="$(var rate)"/>
     <param name="speed_rate" value="$(var speed_rate)"/>
+    <param name="time_start_from_zero" value="$(var time_start_from_zero)"/>
   </node>
 </launch>
diff --git a/src/clock_publisher.cpp b/src/clock_publisher.cpp
index d3ab57d..4877cf5 100644
--- a/src/clock_publisher.cpp
+++ b/src/clock_publisher.cpp
@@ -19,7 +19,12 @@
 void ClockPublisher::onTimer()
 {
   const rclcpp::Duration dif_time = (this->now() - base_time_) * speed_rate_;
-  const rclcpp::Time current_time = base_time_ + dif_time;
+  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.set__clock(current_time);
   clock_pub_->publish(clock);
@@ -31,7 +36,8 @@ ClockPublisher::ClockPublisher(const rclcpp::NodeOptions & node_options)
   base_time_ = this->now();
   clock_pub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
   const int rate = this->declare_parameter("rate", 200);
-  speed_rate_ = this->declare_parameter("speed_rate", 1.0);
+  speed_rate_ = this->declare_parameter<double>("speed_rate", 1.0);
+  time_start_from_zero_ = this->declare_parameter<bool>("time_start_from_zero", false);
 
   // Timer
   auto timer_callback = std::bind(&ClockPublisher::onTimer, this);