forked from Jconn/ros2_android_cpp
-
Notifications
You must be signed in to change notification settings - Fork 0
/
mag_measure.py
49 lines (40 loc) · 1.35 KB
/
mag_measure.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
import rclpy
from rclpy.node import Node
import sensor_msgs.msg
from rclpy.qos import qos_profile_sensor_data
import logging
import numpy as np
class MagLoggerNode(Node):
def __init__(self):
super().__init__("mag_logger_node")
self.mag_sub = self.create_subscription(sensor_msgs.msg.MagneticField,"imu/mag_data", self.mag_data,qos_profile_sensor_data)
self.stats = {}
self.stats['x'] = []
self.stats['y'] = []
self.stats['z'] = []
def mag_data(self, mag_data):
self.stats['x'].append(mag_data.magnetic_field.x)
self.stats['y'].append(mag_data.magnetic_field.y)
self.stats['z'].append(mag_data.magnetic_field.z)
#self.spit_stats()
def spit_stats(self):
for key,val in self.stats.items():
if len(val) == 0:
continue
if len(val) > 100:
val = val[1:]
logging.info(f"stat {key} has std_dev: {np.std(val)}")
def main(args=None):
logging.basicConfig(
format='%(asctime)s %(levelname)-8s %(message)s',
level=logging.INFO,
datefmt='%Y-%m-%d %H:%M:%S')
rclpy.init(args=args)
log_node = MagLoggerNode()
try:
rclpy.spin(log_node)
except:
logging.info(f"hit exception")
log_node.spit_stats()
if __name__ == '__main__':
main()