Common: Time supports rostime mode

modules/common/time/BUILD

@@ -9,6 +9,8 @@ cc_library(
     ],
     deps = [
         "//modules/common:macro",
+        "//modules/common:log",
+        "@ros//:ros_common",
     ],
 )
 

modules/common/time/time.h

@@ -29,7 +29,9 @@
 #include <stdexcept>
 #include <type_traits>
 
+#include "modules/common/log.h"
 #include "modules/common/macro.h"
+#include "ros/include/ros/ros.h"
 
 /**
  * @namespace apollo::common::time
@@ -120,7 +122,6 @@ inline Timestamp FromInt64(int64_t timestamp_value) {
  * a unit of seconds.
  * @return a Timestamp object.
 */
-
 inline Timestamp From(double timestamp_value) {
   int64_t nanos_value = static_cast<int64_t>(timestamp_value * 1e9);
   return FromInt64<nanos>(nanos_value);
@@ -144,12 +145,29 @@ class Clock {
                 "The precision of the system clock should be at least 1 "
                 "microsecond.");
 
+  // The clock mode can either be a system clock time, a user mocked time (for
+  // test only) or read from ROS.
+  enum ClockMode {
+    SYSTEM = 0,
+    MOCK = 1,
+    ROS = 2,
+  };
+
   /**
    * @brief gets the current timestamp.
    * @return a Timestamp object representing the current time.
    */
   static Timestamp Now() {
-    return instance()->is_system_clock_ ? SystemNow() : instance()->mock_now_;
+    switch (mode()) {
+      case ClockMode::SYSTEM:
+        return SystemNow();
+      case ClockMode::MOCK:
+        return instance()->mock_now_;
+      case ClockMode::ROS:
+        return From(ros::Time::now().toSec());
+      default:
+        AFATAL << "Unsupported clock mode: " << mode();
+    }
   }
 
   /**
@@ -162,20 +180,18 @@ class Clock {
 
   /**
    * @brief Set the behavior of the \class Clock.
-   * @param is_system_clock if provided with value TRUE, further call
-   * to Now() will return timestamp based on the system clock. If
-   * provided with FALSE, it will use the mock clock instead.
+   * @param The new clock mode to be set.
    */
-  static void UseSystemClock(bool is_system_clock) {
-    instance()->is_system_clock_ = is_system_clock;
+  static void SetMode(ClockMode mode) {
+    instance()->mode_ = mode;
   }
 
   /**
-   * @brief Check whether the \class Clock instance is using system clock.
-   * @return TRUE if it is using system clock, and FALSE otherwise.
+   * @brief Gets the current clock mode.
+   * @return The current clock mode.
    */
-  static bool IsSystemClock() {
-    return instance()->is_system_clock_;
+  static ClockMode mode() {
+    return instance()->mode_;
   }
 
   /**
@@ -184,8 +200,8 @@ class Clock {
    */
   static void SetNow(const Duration &duration) {
     Clock *clock = instance();
-    if (clock->is_system_clock_) {
-      throw std::runtime_error("Cannot set now when using system clock!");
+    if (clock->mode_ != ClockMode::MOCK) {
+      AFATAL << "Cannot set now when clock mode is not MOCK!";
     }
     clock->mock_now_ = Timestamp(duration);
   }
@@ -193,10 +209,11 @@ class Clock {
  private:
   /**
    * @brief constructs the \class Clock instance
-   * @param is_system_clock See UseSystemClock.
+   * @param mode the desired clock mode
    */
-  explicit Clock(bool is_system_clock)
-      : is_system_clock_(is_system_clock), mock_now_(Timestamp()) {}
+  explicit Clock(ClockMode mode) : mode_(mode), mock_now_(Timestamp()) {
+    ros::Time::init();
+  }
 
   /**
    * @brief Returns the current timestamp based on the system clock.
@@ -207,13 +224,13 @@ class Clock {
         std::chrono::system_clock::now());
   }
 
-  /// NOTE: Unless is_system_clock_ and mock_now_ are guarded by a
+  /// NOTE: Unless mode_ and mock_now_ are guarded by a
   /// lock or become atomic, having multiple threads calling mock
   /// clock related functions are STRICTLY PROHIBITED.
 
   /// Indicates whether it is in the system clock mode or the mock
-  /// clock mode.
-  bool is_system_clock_;
+  /// clock mode or the ROS time mode.
+  ClockMode mode_;
 
   /// Stores the currently set timestamp, which serves mock clock
   /// queries.
@@ -223,7 +240,7 @@ class Clock {
   DECLARE_SINGLETON(Clock);
 };
 
-inline Clock::Clock() : Clock(true) {}
+inline Clock::Clock() : Clock(ClockMode::SYSTEM) {}
 
 // Measure run time of a code block, for debugging puprpose only, don't check in
 // code with this macro!

modules/common/time/time_test.cc

@@ -54,9 +54,9 @@ TEST(TimeTest, TimestampFromAndToDouble) {
 }
 
 TEST(TimeTest, MockTime) {
-  EXPECT_TRUE(Clock::IsSystemClock());
-  Clock::UseSystemClock(false);
-  EXPECT_FALSE(Clock::IsSystemClock());
+  EXPECT_EQ(Clock::SYSTEM, Clock::mode());
+  Clock::SetMode(Clock::MOCK);
+  EXPECT_EQ(Clock::MOCK, Clock::mode());
 
   EXPECT_EQ(0, AsInt64<micros>(Clock::Now()));
   Clock::SetNow(micros(123));

modules/dreamview/backend/sim_control/sim_control_test.cc

@@ -128,7 +128,7 @@ TEST_F(SimControlTest, Test) {
   AdapterManager::GetPlanning()->OnReceive(adc_trajectory);
 
   {
-    Clock::UseSystemClock(false);
+    Clock::SetMode(Clock::MOCK);
     const auto timestamp = apollo::common::time::From(100.01);
     Clock::SetNow(timestamp.time_since_epoch());
     sim_control_.TimerCallback(ros::TimerEvent());