2 from __future__
import print_function, division
6 from dynamic_reconfigure.server
import Server
7 from hector_timestamp_alignment.cfg
import timestamp_alignmentConfig
11 self.
imu_sub = rospy.Subscriber(
"/imu/data_original", sensor_msgs.msg.Imu, self.
imu_cb)
12 self.
imu_aligned_pub = rospy.Publisher(
"/imu/data", sensor_msgs.msg.Imu, queue_size=10)
18 imu_msg.header.stamp = imu_msg.header.stamp + rospy.Duration(self.
delay)
20 self.imu_aligned_pub.publish(imu_msg)
23 print(config.imu_offset)
24 self.
delay = config.imu_offset
28 if __name__ ==
"__main__":
29 rospy.init_node(
"timestamp_alignment_node")
31 while not rospy.is_shutdown():
def imu_cb(self, imu_msg)
def __init__(self, verbose)
def dynamic_reconfigure_cb(self, config, level)