Time Synchronization

This function (https://github.com/RobustFieldAutonomyLab/bluerov/commit/df5509b20be00a5d19c9cf2d138f75cf101984e6) implements the algorithm in https://github.com/ros-drivers/urg_node/blob/d2722c60f1b4713bbe1d39f32849090dece0104d/src/urg_c_wrapper.cpp#L1052, which is to synchronize the time between hardware and system (ros::Time). A more advanced algorithm could be added https://github.com/ethz-asl/cuckoo_time_translator.

def sync_time(time_stamp, system_time_stamp):
    global hardware_clock, last_hardware_time_stamp, hardware_clock_adj, adj_count
    delta = time_stamp - last_hardware_time_stamp
    hardware_clock += delta
    cur_adj = system_time_stamp.to_sec() - hardware_clock
    if adj_count > 0:
        hardware_clock_adj = adj_alpha * cur_adj + (1.0 - adj_alpha) * hardware_clock_adj
    else:
        hardware_clock_adj = cur_adj
    
    adj_count += 1
    last_hardware_time_stamp = time_stamp
    
    stamp = system_time_stamp
    if adj_count > 100:
        stamp = stamp.from_sec(hardware_clock + hardware_clock_adj)
        
        if abs((stamp - system_time_stamp).to_sec()) > 0.1:
            adj_count = 0
            hardware_clock = 0.0
            last_hardware_time_stamp = 0.0
            stamp = system_time_stamp
    return stamp

DVL

IMU

Oculus Sonar

The hardware clock of Oculus sonar is not stable.