dataglove/node_lsm.py at main · francescapoli98/dataglove · GitHub

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
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#!/usr/bin/env python3

import rospy
import torch
import os
import traceback
import inspect
import numpy as np
from joblib import load
import json
import csv
from dataglove.msg import ClassificationData
from sklearn.preprocessing import StandardScaler
from sklearn.linear_model import LogisticRegression
from classification.lsm import LiquidStateMachine
import rospkg

class LSMNode:
    def __init__(self):
        self.batch_size = rospy.get_param('/dataglove_params/buffer_size', 50)
        self.input_size = 21
        self.started = False
        self.mix_subscriber = None

        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('dataglove')
        model_dir = os.path.join(pkg_path, 'models')

        ckpt_path = os.path.join(model_dir, "lsm_checkpoint.pt")
        checkpoint = torch.load(ckpt_path, map_location='cpu', weights_only=False)
        filtered_config = self.filter_model_args(LiquidStateMachine, checkpoint['config'])
        self.model = LiquidStateMachine(**filtered_config)
        self.model.load_state_dict(checkpoint['model_state_dict'])
        self.model.eval()

        self.scaler = load(os.path.join(model_dir, "lsm_scaler.joblib"))
        self.classifier = load(os.path.join(model_dir, "lsm_classifier.joblib"))

        with open(os.path.join(model_dir, "label_map.json"), "r") as f:
            self.label_map = json.load(f)

        self.log_file_path = os.path.join(os.path.expanduser('~'), 'lsm_latency_log.txt')
        self.predtime_file_path = os.path.join(os.path.expanduser('~'), 'lsm_predict_lat.txt')
        self.last_label = None  # Store last prediction

        # Subscribe to your custom message with header and data[]
        self.mix_subscriber = rospy.Subscriber("/glove_buffer", ClassificationData, self.buffer_callback)
        rospy.loginfo(f"[LSM] Starting node")


    @staticmethod
    def filter_model_args(model_class, config_dict):
        valid_args = set(inspect.signature(model_class.__init__).parameters.keys()) - {'self'}
        return {k: v for k, v in config_dict.items() if k in valid_args}

    @staticmethod
    def append_to_csv(pred, label, file_path):
        timestamp = rospy.Time.now()
        with open(file_path, mode='a', newline='') as f:
            writer = csv.writer(f)
            writer.writerow([timestamp, pred, label])


    def buffer_callback(self, msg):
        try:
            # Calculate latency
            now = rospy.Time.now()
            latency = (now - msg.header.stamp).to_sec() * 1000.0  # in milliseconds
            with open(self.log_file_path, 'a') as f:
                f.write(f"{latency:.3f}\n")

            flat_data = np.array(msg.data, dtype=np.float32)
            buffer = flat_data.reshape((self.batch_size, self.input_size))

            palm_arch_values = buffer[:, 10]
            if not np.any(palm_arch_values > 0):
                return

            input_tensor = torch.tensor(buffer, dtype=torch.float32)

            with torch.no_grad():
                model_output = self.model(input_tensor)[0]
                if isinstance(model_output, list):
                    model_output = model_output[0]

                output = model_output[-1, :]  # (256,)
                output_np = output.numpy().reshape(1, -1)

                norm_output = self.scaler.transform(output_np)
                ##start time
                before_pred = rospy.Time.now()
                pred = self.classifier.predict(norm_output)[0]
                ## end time
                after_pred = rospy.Time.now()
                pred_time = (after_pred - before_pred).to_sec() * 1000.0  # in milliseconds
                with open(self.predtime_file_path, 'a') as f:
                    f.write(f"{(pred_time):.3f}\n")

                label = self.label_map.get(str(pred), pred)
                self.append_to_csv(pred, label, os.path.join(os.path.expanduser('~'), 'lsm_predictions_hardbottle.csv'))
                if str(label) != str(self.last_label):
                    rospy.loginfo(f"[LSM] Predicted label: {label}")
                self.last_label = label  # Update last prediction

        except Exception as e:
            rospy.logerr(f"[LSM] Error in callback: {e}")
            rospy.logerr("[LSM] Full traceback:\n%s", traceback.format_exc())

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    try:
        rospy.init_node("node_lsm")
        node = LSMNode()
        node.run()
    except rospy.ROSInterruptException:
        pass