Add options to clock_publlisher

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,9 @@ 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_;
+  bool time_start_from_zero_;
 };
 
 #endif

launch/clock_publisher.launch.xml

@@ -1,6 +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>

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<rosgraph_msgs::msg::Clock>("/clock", 1);
   const int rate = this->declare_parameter("rate", 200);
+  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);
   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),