added the capability to work with non sim time in timekeeper

avidbots · Mar 14, 2024 · b19877e · b19877e
1 parent b6cece7
commit b19877e
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 4 deletions.
diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch
@@ -13,8 +13,6 @@
 
   <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
 
-  <param name="use_sim_time" value="true"/>  
-
   <!-- launch flatland server -->
   <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
     <!-- Use the arguments passed into the launchfile for this node -->

diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp
@@ -49,8 +49,17 @@
 
 namespace flatland_server {
 
-Timekeeper::Timekeeper()
-    : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") {
+Timekeeper::Timekeeper() : max_step_size_(0), clock_topic_("/clock") {
+  bool use_sim_time = false;
+  nh_.getParam("use_sim_time", use_sim_time);
+
+  if (!use_sim_time) {
+    time_ =
+        ros::Time(ros::WallTime::now().toSec(), ros::WallTime::now().toNSec());
+  } else {
+    time_ = ros::Time(0, 0);
+  }
+
   clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>(clock_topic_, 1);
 }