timestamp_alignment.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 from __future__ import print_function, division
00003 import rospy
00004 
00005 import sensor_msgs.msg
00006 from dynamic_reconfigure.server import Server
00007 from hector_timestamp_alignment.cfg import timestamp_alignmentConfig
00008 
00009 class TimestampAlignment:
00010     def __init__(self, verbose):
00011         self.imu_sub = rospy.Subscriber("/imu/data_original", sensor_msgs.msg.Imu, self.imu_cb)
00012         self.imu_aligned_pub = rospy.Publisher("/imu/data", sensor_msgs.msg.Imu, queue_size=10)
00013         self.srv = Server(timestamp_alignmentConfig, self.dynamic_reconfigure_cb)
00014         self.delay = 0.00
00015 
00016     def imu_cb(self, imu_msg):
00017         #print("Before: ", imu_msg.header.stamp)
00018         imu_msg.header.stamp = imu_msg.header.stamp + rospy.Duration(self.delay)
00019         #print("After: ", imu_msg.header.stamp)
00020         self.imu_aligned_pub.publish(imu_msg)
00021 
00022     def dynamic_reconfigure_cb(self, config, level):
00023         print(config.imu_offset)
00024         self.delay = config.imu_offset
00025         return config
00026 
00027 
00028 if __name__ == "__main__":
00029     rospy.init_node("timestamp_alignment_node")
00030     hazmat_detection = TimestampAlignment(verbose=False)
00031     while not rospy.is_shutdown():
00032         rospy.spin()


hector_timestamp_alignment
Author(s):
autogenerated on Wed May 8 2019 02:32:19