You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
50 lines
1.5 KiB
50 lines
1.5 KiB
#!/usr/bin/env python
|
|
|
|
from __future__ import print_function
|
|
|
|
import sys
|
|
import rospy
|
|
from sensor_msgs.msg import Imu
|
|
|
|
class D435iAccelCorrectionNode:
|
|
def __init__(self):
|
|
self.num_samples_to_skip = 4
|
|
self.sample_count = 0
|
|
|
|
def accel_callback(self, accel):
|
|
self.accel = accel
|
|
self.sample_count += 1
|
|
if (self.sample_count % self.num_samples_to_skip) == 0:
|
|
# This can avoid issues with the D435i's time stamps being too
|
|
# far ahead for TF.
|
|
self.accel.header.stamp = rospy.Time.now()
|
|
x = self.accel.linear_acceleration.x
|
|
y = self.accel.linear_acceleration.y
|
|
z = self.accel.linear_acceleration.z
|
|
|
|
self.accel.linear_acceleration.x = x
|
|
self.accel.linear_acceleration.y = y
|
|
self.accel.linear_acceleration.z = z
|
|
|
|
self.corrected_accel_pub.publish(self.accel)
|
|
|
|
|
|
def main(self):
|
|
rospy.init_node('D435iAccelCorrectionNode')
|
|
self.node_name = rospy.get_name()
|
|
rospy.loginfo("{0} started".format(self.node_name))
|
|
|
|
self.topic_name = '/camera/accel/sample'
|
|
self.accel_subscriber = rospy.Subscriber(self.topic_name, Imu, self.accel_callback)
|
|
|
|
self.corrected_accel_pub = rospy.Publisher('/camera/accel/sample_corrected', Imu, queue_size=1)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
node = D435iAccelCorrectionNode()
|
|
node.main()
|
|
try:
|
|
rospy.spin()
|
|
except KeyboardInterrupt:
|
|
print('interrupt received, so shutting down')
|
|
|