Merge branch 'feature-timesync'

Author: jinkunw

imu_vn_100/include/imu_vn_100/imu_vn_100.h

@@ -114,6 +114,16 @@ class ImuVn100 {
 
   void FixImuRate();
   void LoadParameters();
+
+  // used for clock synchronziation
+  bool synchronize_time_;
+  double hardware_clock_;
+  uint64_t last_hardware_time_stamp_;
+  double hardware_clock_adj_;
+  const double adj_alpha_ = .01;
+  uint64_t adj_count_;
+
+  ros::Time getSynchronizedTime(uint64_t time_stamp, const ros::Time &system_time);
 };
 
 // Just don't like type that is ALL CAP

imu_vn_100/src/imu_vn_100.cpp

@@ -420,7 +420,8 @@ void ImuVn100::Disconnect() {
 
 void ImuVn100::PublishData(const VnDeviceCompositeData& data) {
   sensor_msgs::Imu imu_msg;
-  imu_msg.header.stamp = ros::Time::now();
+  // imu_msg.header.stamp = ros::Time::now();
+  imu_msg.header.stamp = getSynchronizedTime(data.timeStartup, ros::Time::now());
   imu_msg.header.frame_id = frame_id_;
 
   if (imu_compensated_) {
@@ -471,6 +472,45 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) {
   // updater_.update();
 }
 
+ros::Time ImuVn100::getSynchronizedTime(uint64_t time_stamp, const ros::Time &system_time)
+{
+  ros::Time stamp = system_time;
+
+  double delta = (time_stamp - last_hardware_time_stamp_) * 1.0e-9;
+  hardware_clock_ += delta;
+  double cur_adj = stamp.toSec() - hardware_clock_;
+  if (adj_count_ > 0)
+  {
+    hardware_clock_adj_ = adj_alpha_*cur_adj+(1.0-adj_alpha_)*hardware_clock_adj_;
+  }
+  else
+  {
+    // Initialize the EMA
+    hardware_clock_adj_ = cur_adj;
+  }
+  adj_count_++;
+  last_hardware_time_stamp_ = time_stamp;
+
+  // Once hardware clock is synchronized, use it. Otherwise just return the
+  // input system_time_stamp as ros::Time.
+  if (adj_count_ > 100)
+  {
+    stamp.fromSec(hardware_clock_+hardware_clock_adj_);
+    // If the time error is large a clock warp has occurred.
+    // Reset the EMA and use the system time.
+    if (fabs((stamp-system_time).toSec()) > 0.1)
+    {
+        adj_count_ = 0;
+        hardware_clock_ = 0.0;
+        last_hardware_time_stamp_ = 0;
+        stamp = system_time;
+        ROS_INFO("%s: detected clock warp, reset EMA", __func__);
+    }
+  }
+  return stamp;
+}
+
+
 void VnEnsure(const VnErrorCode& error_code) {
   if (error_code == VNERR_NO_ERROR) return;
 

rti_dvl/scripts/rti_dvl_node.py

@@ -9,25 +9,66 @@
 
 SALINITY = {'fresh': 0, 'salt': 35}
 
-def zeor_pressure(req):
-    rospy.loginfo('Zero pressure sensor')
 
-    dvl.write('CPZ\r\n')
-    return TriggerResponse(success=True)
+class SyncTime(object):
+    """Time translator
+    - See https://github.com/ethz-asl/cuckoo_time_translator for more details.
+    - Implement https://github.com/ros-drivers/urg_node/blob/d2722c60f1b4713bbe1d39f32849090dece0104d/src/urg_c_wrapper.cpp#L1052
+    """
+
+    def __init__(self):
+        self.hardware_clock = 0.0
+        self.last_hardware_time_stamp = 0.0
+        self.hardware_clock_adj = 0.0
+        self.adj_count = 0
+        self.adj_alpha = 0.01
+
+    def sync(self, time_stamp, system_time_stamp):
+        delta = time_stamp - self.last_hardware_time_stamp
+        self.hardware_clock += delta
+        cur_adj = system_time_stamp.to_sec() - self.hardware_clock
+        if self.adj_count > 0:
+            self.hardware_clock_adj = (
+                self.adj_alpha * cur_adj
+                + (1.0 - self.adj_alpha) * self.hardware_clock_adj
+            )
+        else:
+            self.hardware_clock_adj = cur_adj
 
+        self.adj_count += 1
+        self.last_hardware_time_stamp = time_stamp
 
-def toggle_status(req):
-    global running
-    if running:
-        rospy.loginfo('Stop DVL')
-        dvl.write('STOP\r\n')
-        running = False
-        return TriggerResponse(success=True, message='Stop')
-    else:
-        rospy.loginfo('Start DVL')
-        dvl.write('START\r\n')
-        running = True
-        return TriggerResponse(success=True, message='Start')
+        stamp = system_time_stamp
+        if self.adj_count > 100:
+            stamp = stamp.from_sec(self.hardware_clock + self.hardware_clock_adj)
+
+            if abs((stamp - system_time_stamp).to_sec()) > 0.1:
+                self.adj_count = 0
+                self.hardware_clock = 0.0
+                self.last_hardware_time_stamp = 0.0
+                self.stamp = system_time_stamp
+        return stamp
+
+
+# def zeor_pressure(req):
+#     rospy.loginfo('Zero pressure sensor')
+
+#     dvl.write('CPZ\r\n')
+#     return TriggerResponse(success=True)
+
+
+# def toggle_status(req):
+#     global running
+#     if running:
+#         rospy.loginfo('Stop DVL')
+#         dvl.write('STOP\r\n')
+#         running = False
+#         return TriggerResponse(success=True, message='Stop')
+#     else:
+#         rospy.loginfo('Start DVL')
+#         dvl.write('START\r\n')
+#         running = True
+#         return TriggerResponse(success=True, message='Start')
 
 
 if __name__ == '__main__':
@@ -40,6 +81,8 @@ def toggle_status(req):
     water = rospy.get_param('/water', 'fresh')
     salinity = SALINITY[water]
 
+    rospy.logwarn('Water is {}. Please set salinity in DVL.'.format(water))
+
     # TODO
     # Change salinity here
 
@@ -122,15 +165,19 @@ def toggle_status(req):
     dvl_pub = rospy.Publisher('/rti/body_velocity/raw', DVL, queue_size=100)
     reader = pynmea2.NMEAStreamReader(errors='ignore')
 
+    sync = SyncTime()
     while not rospy.is_shutdown():
         try:
             char = dvl.read()
         except serial.SerialException:
             break
         for msg in reader.next(char):
-            if isinstance(msg, pynmea2.types.rti.RTI01) or isinstance(msg, pynmea2.types.rti.RTI03):
+            if isinstance(msg, pynmea2.types.rti.RTI01) or isinstance(
+                msg, pynmea2.types.rti.RTI03
+            ):
                 d = DVL()
-                d.header.stamp = rospy.Time.now()
+                stamp = sync.sync(msg.time / 100.0, rospy.Time.now())
+                d.header.stamp = stamp
                 d.velocity.x = msg.x / 1000.0
                 d.velocity.y = msg.y / 1000.0
                 d.velocity.z = msg.z / 1000.0