import rclpy
from rclpy.node import Node
import can
import math
import os
import yaml
from sensor_msgs.msg import JointState
from rcl_interfaces.msg import SetParametersResult

class CallbackListener(can.Listener):
    def __init__(self, user_callback):
        super().__init__()
        self.user_callback = user_callback

    def on_message_received(self, msg: can.Message):
        # 这是在 Notifier 的线程里调用的 -> 回调应该尽量快
        try:
            self.user_callback(msg)
        except Exception as e:
            # 捕获回调内异常，避免中断 Notifier 分发
            print(f"Callback error: {e}")


class JointSensorNode(Node):
    def __init__(self):
        super().__init__('joint_sensor_node')
        self.num_joints = 8
        self.arm_labels = {'right': 'RIGHT', 'left': 'LEFT'}

        # CAN interface and topic parameters
        self.declare_parameter('right_can_interface', 'can2')
        self.declare_parameter('left_can_interface', 'can3')
        self.declare_parameter('enable_left_arm', True)
        self.declare_parameter('right_joint_states_topic', 'right_joint_sensor_states')
        self.declare_parameter('left_joint_states_topic', 'left_joint_sensor_states')

        # Calibration and direction parameters
        self.declare_parameter('right_zero_offsets', [0.0] * self.num_joints)
        self.declare_parameter('right_sensor_directions', [1.0] * self.num_joints)
        self.declare_parameter('left_zero_offsets', [0.0] * self.num_joints)
        self.declare_parameter('left_sensor_directions', [1.0] * self.num_joints)
        self.declare_parameter('right_auto_zero_on_first_sample', True)
        self.declare_parameter('left_auto_zero_on_first_sample', True)
        self.declare_parameter('right_calibrate_now', False)
        self.declare_parameter('left_calibrate_now', False)
        default_yaml_path = os.path.normpath(
            os.path.join(os.path.dirname(__file__), '..', 'config', 'joint_sensor_zero_offsets.yaml')
        )
        if not os.path.exists(default_yaml_path):
            default_yaml_path = ''
        self.declare_parameter('calibration_yaml_path', default_yaml_path)
        self.declare_parameter('save_calibration_now', False)
        self.declare_parameter('auto_save_yaml_on_calibrate', True)

        # Diagnostics and anomaly filters
        self.declare_parameter('enable_speed_jump_log', False)
        self.declare_parameter('speed_jump_threshold_rad_s', 3.0)
        self.declare_parameter('jump_log_min_interval_sec', 0.2)
        self.declare_parameter('joint_update_epsilon_rad', 1e-4)
        self.declare_parameter('enable_stage_diag_log', False)
        self.declare_parameter('stage_log_min_interval_sec', 0.5)
        self.declare_parameter('enable_raw_jump_log', False)
        self.declare_parameter('raw_jump_threshold_rad', 0.2)
        self.declare_parameter('raw_jump_log_min_interval_sec', 0.2)
        self.declare_parameter('enable_raw_jump_reject', True)
        self.declare_parameter('raw_jump_reject_threshold_rad', 0.40)

        # Runtime flags
        self.enable_left_arm = bool(self.get_parameter('enable_left_arm').value)
        self.enable_speed_jump_log = bool(self.get_parameter('enable_speed_jump_log').value)
        self.speed_jump_threshold_rad_s = float(self.get_parameter('speed_jump_threshold_rad_s').value)
        self.jump_log_min_interval_sec = float(self.get_parameter('jump_log_min_interval_sec').value)
        self.joint_update_epsilon_rad = float(self.get_parameter('joint_update_epsilon_rad').value)
        self.enable_stage_diag_log = bool(self.get_parameter('enable_stage_diag_log').value)
        self.stage_log_min_interval_sec = float(self.get_parameter('stage_log_min_interval_sec').value)
        self.enable_raw_jump_log = bool(self.get_parameter('enable_raw_jump_log').value)
        self.raw_jump_threshold_rad = float(self.get_parameter('raw_jump_threshold_rad').value)
        self.raw_jump_log_min_interval_sec = float(self.get_parameter('raw_jump_log_min_interval_sec').value)
        self.enable_raw_jump_reject = bool(self.get_parameter('enable_raw_jump_reject').value)
        self.raw_jump_reject_threshold_rad = float(self.get_parameter('raw_jump_reject_threshold_rad').value)
        self.calibration_yaml_path = str(self.get_parameter('calibration_yaml_path').value)
        self.auto_save_yaml_on_calibrate = bool(self.get_parameter('auto_save_yaml_on_calibrate').value)

        # Sensor unit setting
        self.raw_angle_unit = 'centi-rad'

        self.get_logger().info('正在初始化CAN接口...')

        right_can_interface = str(self.get_parameter('right_can_interface').value)
        left_can_interface = str(self.get_parameter('left_can_interface').value)
        right_topic = str(self.get_parameter('right_joint_states_topic').value)
        left_topic = str(self.get_parameter('left_joint_states_topic').value)

        self.joint_names = [f'joint{i}' for i in range(1, self.num_joints + 1)]
        self.can_to_joint_index = {
            0x01: 0,
            0x02: 1,
            0x03: 2,
            0x04: 3,
            0x05: 4,
            0x06: 5,
            0x07: 6,
            0x08: 7,
        }

        self.arms = {
            'right': self._new_arm_state(
                zero_offsets=self._load_offsets_param('right_zero_offsets'),
                sensor_directions=self._load_directions_param('right_sensor_directions'),
                auto_zero=bool(self.get_parameter('right_auto_zero_on_first_sample').value),
                publisher=self.create_publisher(JointState, right_topic, 10),
                bus=can.interface.Bus(channel=right_can_interface, bustype='socketcan'),
            )
        }
        if self.enable_left_arm:
            self.arms['left'] = self._new_arm_state(
                zero_offsets=self._load_offsets_param('left_zero_offsets'),
                sensor_directions=self._load_directions_param('left_sensor_directions'),
                auto_zero=bool(self.get_parameter('left_auto_zero_on_first_sample').value),
                publisher=self.create_publisher(JointState, left_topic, 10),
                bus=can.interface.Bus(channel=left_can_interface, bustype='socketcan'),
            )

        self.add_on_set_parameters_callback(self.on_set_parameters)

        self.notifiers = []
        for arm_name, state in self.arms.items():
            listener = CallbackListener(lambda msg, a=arm_name: self.listen_can_callback(msg, a))
            notifier = can.Notifier(state['bus'], [listener])
            state['listener'] = listener
            state['notifier'] = notifier
            self.notifiers.append(notifier)

    def _new_arm_state(self, zero_offsets, sensor_directions, auto_zero, publisher, bus):
        return {
            'publisher': publisher,
            'bus': bus,
            'notifier': None,
            'listener': None,
            'positions': [0.0] * self.num_joints,
            'velocities': [0.0] * self.num_joints,
            'has_position': [False] * self.num_joints,
            'last_wrapped_positions': [0.0] * self.num_joints,
            'has_wrapped_position': [False] * self.num_joints,
            'last_joint_update_sec': [None] * self.num_joints,
            'last_jump_log_sec': [0.0] * self.num_joints,
            'last_stage_log_sec': [0.0] * self.num_joints,
            'prev_raw_sensor_angle': [None] * self.num_joints,
            'last_raw_jump_log_sec': [0.0] * self.num_joints,
            'last_raw_reject_log_sec': [0.0] * self.num_joints,
            'zero_offsets': zero_offsets,
            'sensor_directions': sensor_directions,
            'auto_zero_on_first_sample': auto_zero,
            'auto_zero_done': [False] * self.num_joints,
        }
    
    def listen_can_callback(self, msg: can.Message, arm_name: str):
        # 用户自定义的回调函数
        try:
            state = self.arms.get(arm_name)
            if state is None:
                return

            joint_index = self.can_to_joint_index.get(msg.arbitration_id)
            if joint_index is None:
                self.get_logger().debug(f"忽略未映射CAN ID: 0x{msg.arbitration_id:X}")
                return

            msg_time_sec = float(msg.timestamp) if msg.timestamp is not None else (self.get_clock().now().nanoseconds * 1e-9)
            raw_sensor_angle = self.extract_raw_sensor_angle_rad(msg.data)

            # 原始数据跳变过大时直接丢弃该帧，避免异常值进入后续链路
            if self._should_reject_raw_jump(state, arm_name, joint_index, raw_sensor_angle, msg_time_sec, msg):
                return

            self._log_raw_jump(state, arm_name, joint_index, raw_sensor_angle, msg_time_sec, msg)

            # 每条CAN报文默认对应一个关节角和速度
            parsed_angle, vel = self.parse_sensor_from_can(msg.data)
            direction = state['sensor_directions'][joint_index]
            directed_angle = self.wrap_to_pi(parsed_angle * direction)
            vel = vel * direction

            state['last_wrapped_positions'][joint_index] = directed_angle
            state['has_wrapped_position'][joint_index] = True

            if state['auto_zero_on_first_sample'] and not state['auto_zero_done'][joint_index]:
                state['zero_offsets'][joint_index] = directed_angle
                state['auto_zero_done'][joint_index] = True

            zeroed_angle = self.wrap_to_pi(directed_angle - state['zero_offsets'][joint_index])
            angle = self.unwrap_joint_angle(joint_index, zeroed_angle, state['positions'], state['has_position'])

            self._log_stage_diag(
                state,
                arm_name,
                joint_index,
                parsed_angle,
                directed_angle,
                zeroed_angle,
                angle,
                vel,
                msg_time_sec,
            )
            self._log_speed_jump(state, arm_name, joint_index, angle, msg_time_sec)

            state['positions'][joint_index] = angle
            state['velocities'][joint_index] = vel

            # 发布完整8关节状态（位置/速度单位均为弧度）
            js = JointState()
            js.header.stamp = self.get_clock().now().to_msg()
            js.name = self.joint_names
            js.position = state['positions']
            js.velocity = state['velocities']
            state['publisher'].publish(js)
        except Exception as e:
            self.get_logger().error(f"Error handling CAN msg: {e}")

    def shutdown(self):
        for state in self.arms.values():
            if state['notifier'] is not None:
                state['notifier'].stop()
            if state['bus'] is not None:
                state['bus'].shutdown()

    def _load_offsets_param(self, name):
        raw_offsets = self.get_parameter(name).value
        if not isinstance(raw_offsets, (list, tuple)) or len(raw_offsets) != self.num_joints:
            self.get_logger().warn(f'参数 {name} 长度不是{self.num_joints},已使用默认零偏。')
            return [0.0] * self.num_joints
        return [float(v) for v in raw_offsets]

    def _load_directions_param(self, name):
        raw_dirs = self.get_parameter(name).value
        if not isinstance(raw_dirs, (list, tuple)) or len(raw_dirs) != self.num_joints:
            self.get_logger().warn(f'参数 {name} 长度不是{self.num_joints},已使用默认方向。')
            return [1.0] * self.num_joints

        dirs = [float(v) for v in raw_dirs]
        for i, v in enumerate(dirs):
            if abs(v) < 1e-9:
                self.get_logger().warn(f'sensor_directions[{i}] 不能为0，已回退为1.0')
                dirs[i] = 1.0
        return dirs

    def calibrate_zero_from_current(self, arm_name):
        state = self.arms.get(arm_name)
        if state is None:
            return

        param_name = 'left_zero_offsets' if arm_name == 'left' else 'zero_offsets'
        updated = 0
        for i in range(self.num_joints):
            if state['has_wrapped_position'][i]:
                state['zero_offsets'][i] = state['last_wrapped_positions'][i]
                state['has_position'][i] = False
                updated += 1

        if updated > 0:
            self.set_parameters([
                rclpy.parameter.Parameter(
                    param_name,
                    rclpy.Parameter.Type.DOUBLE_ARRAY,
                    state['zero_offsets'],
                )
            ])
            self.get_logger().info(f'[{self.arm_labels[arm_name]}] 零点校准完成，已更新 {updated}/{self.num_joints} 个关节零偏。')
            if self.auto_save_yaml_on_calibrate:
                self.save_calibration_to_yaml()
        else:
            self.get_logger().warn('零点校准失败：当前还没有可用的关节采样数据。')

    def save_calibration_to_yaml(self):
        yaml_path = self.calibration_yaml_path
        if not yaml_path:
            self.get_logger().warn('未配置 calibration_yaml_path，无法写回 YAML。')
            return False

        try:
            data = {}
            if os.path.exists(yaml_path):
                with open(yaml_path, 'r', encoding='utf-8') as f:
                    loaded = yaml.safe_load(f)
                    if isinstance(loaded, dict):
                        data = loaded

            node_cfg = data.get('joint_sensor_node')
            if not isinstance(node_cfg, dict):
                node_cfg = {}
                data['joint_sensor_node'] = node_cfg

            ros_params = node_cfg.get('ros__parameters')
            if not isinstance(ros_params, dict):
                ros_params = {}
                node_cfg['ros__parameters'] = ros_params

            right = self.arms.get('right')
            if right is not None:
                ros_params['right_zero_offsets'] = [float(v) for v in right['zero_offsets']]
                ros_params['right_sensor_directions'] = [float(v) for v in right['sensor_directions']]

            left = self.arms.get('left')
            if left is not None:
                ros_params['left_zero_offsets'] = [float(v) for v in left['zero_offsets']]
                ros_params['left_sensor_directions'] = [float(v) for v in left['sensor_directions']]

            os.makedirs(os.path.dirname(yaml_path), exist_ok=True)
            with open(yaml_path, 'w', encoding='utf-8') as f:
                yaml.safe_dump(data, f, allow_unicode=False, sort_keys=False)

            self.get_logger().info(f'已将当前校准值写回 YAML: {yaml_path}')
            return True
        except Exception as e:
            self.get_logger().error(f'写回 YAML 失败: {e}')
            return False

    def on_set_parameters(self, params):
        for param in params:
            if param.name in ('zero_offsets', 'left_zero_offsets'):
                if not isinstance(param.value, (list, tuple)) or len(param.value) != self.num_joints:
                    return SetParametersResult(successful=False, reason=f'{param.name} 必须是长度为{self.num_joints}的数组')

            if param.name in ('sensor_directions', 'left_sensor_directions'):
                if not isinstance(param.value, (list, tuple)) or len(param.value) != self.num_joints:
                    return SetParametersResult(successful=False, reason=f'{param.name} 必须是长度为{self.num_joints}的数组')
                for i, v in enumerate(param.value):
                    if abs(float(v)) < 1e-9:
                        return SetParametersResult(successful=False, reason=f'{param.name}[{i}] 不能为0')

            if param.name == 'right_auto_zero_on_first_sample':
                self.arms['right']['auto_zero_on_first_sample'] = bool(param.value)

            if param.name == 'left_auto_zero_on_first_sample' and 'left' in self.arms:
                self.arms['left']['auto_zero_on_first_sample'] = bool(param.value)

            if param.name == 'calibration_yaml_path':
                self.calibration_yaml_path = str(param.value)

            if param.name == 'auto_save_yaml_on_calibrate':
                self.auto_save_yaml_on_calibrate = bool(param.value)

            if param.name == 'enable_speed_jump_log':
                self.enable_speed_jump_log = bool(param.value)

            if param.name == 'speed_jump_threshold_rad_s':
                self.speed_jump_threshold_rad_s = float(param.value)

            if param.name == 'jump_log_min_interval_sec':
                self.jump_log_min_interval_sec = float(param.value)

            if param.name == 'joint_update_epsilon_rad':
                self.joint_update_epsilon_rad = float(param.value)

            if param.name == 'enable_stage_diag_log':
                self.enable_stage_diag_log = bool(param.value)

            if param.name == 'stage_log_min_interval_sec':
                self.stage_log_min_interval_sec = float(param.value)

            if param.name == 'enable_raw_jump_log':
                self.enable_raw_jump_log = bool(param.value)

            if param.name == 'raw_jump_threshold_rad':
                self.raw_jump_threshold_rad = float(param.value)

            if param.name == 'raw_jump_log_min_interval_sec':
                self.raw_jump_log_min_interval_sec = float(param.value)

            if param.name == 'enable_raw_jump_reject':
                self.enable_raw_jump_reject = bool(param.value)

            if param.name == 'raw_jump_reject_threshold_rad':
                self.raw_jump_reject_threshold_rad = float(param.value)

            if param.name == 'right_calibrate_now' and bool(param.value):
                self.calibrate_zero_from_current('right')

            if param.name == 'left_calibrate_now' and bool(param.value):
                self.calibrate_zero_from_current('left')

            if param.name == 'save_calibration_now' and bool(param.value):
                self.save_calibration_to_yaml()

        for param in params:
            if param.name == 'right_zero_offsets':
                self.arms['right']['zero_offsets'] = [float(v) for v in param.value]
                self.arms['right']['has_position'] = [False] * self.num_joints

            if param.name == 'right_sensor_directions':
                self.arms['right']['sensor_directions'] = [float(v) for v in param.value]
                self.arms['right']['has_position'] = [False] * self.num_joints

            if param.name == 'left_zero_offsets' and 'left' in self.arms:
                self.arms['left']['zero_offsets'] = [float(v) for v in param.value]
                self.arms['left']['has_position'] = [False] * self.num_joints

            if param.name == 'left_sensor_directions' and 'left' in self.arms:
                self.arms['left']['sensor_directions'] = [float(v) for v in param.value]
                self.arms['left']['has_position'] = [False] * self.num_joints

        return SetParametersResult(successful=True)

    def _log_stage_diag(
        self,
        state,
        arm_name,
        joint_index: int,
        parsed_angle: float,
        directed_angle: float,
        zeroed_angle: float,
        unwrapped_angle: float,
        vel: float,
        now_sec: float,
    ):
        if not self.enable_stage_diag_log:
            return

        since_last = now_sec - state['last_stage_log_sec'][joint_index]
        if since_last < self.stage_log_min_interval_sec:
            return

        state['last_stage_log_sec'][joint_index] = now_sec
        self.get_logger().info(
            f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} stages: parsed={parsed_angle:.6f} dir={directed_angle:.6f} "
            f"zeroed={zeroed_angle:.6f} unwrapped={unwrapped_angle:.6f} vel={vel:.6f}"
        )

    def _log_speed_jump(self, state, arm_name, joint_index: int, curr_unwrapped_angle: float, now_sec: float):
        if not self.enable_speed_jump_log:
            state['last_joint_update_sec'][joint_index] = now_sec
            return

        prev_time = state['last_joint_update_sec'][joint_index]
        if prev_time is None:
            state['last_joint_update_sec'][joint_index] = now_sec
            return

        dt = now_sec - prev_time
        if dt <= 1e-6:
            state['last_joint_update_sec'][joint_index] = now_sec
            return

        prev_unwrapped = state['positions'][joint_index]
        delta = curr_unwrapped_angle - prev_unwrapped
        if abs(delta) <= self.joint_update_epsilon_rad:
            state['last_joint_update_sec'][joint_index] = now_sec
            return

        speed = abs(delta) / dt
        if speed >= self.speed_jump_threshold_rad_s:
            since_last_log = now_sec - state['last_jump_log_sec'][joint_index]
            if since_last_log >= self.jump_log_min_interval_sec:
                self.get_logger().warning(
                    f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} speed jump: prev={prev_unwrapped:.6f} curr={curr_unwrapped_angle:.6f} "
                    f"delta={delta:.6f} rad dt={dt:.4f} s speed={speed:.3f} rad/s"
                )
                state['last_jump_log_sec'][joint_index] = now_sec

        state['last_joint_update_sec'][joint_index] = now_sec

    def _log_raw_jump(self, state, arm_name, joint_index: int, raw_sensor_angle: float, now_sec: float, msg: can.Message):
        if not self.enable_raw_jump_log or raw_sensor_angle is None:
            if raw_sensor_angle is not None:
                state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
            return

        prev = state['prev_raw_sensor_angle'][joint_index]
        if prev is None:
            state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
            return

        # 原始角度做最短角距离，避免 0/2pi 边界导致假跳变
        delta = self.wrap_to_pi(raw_sensor_angle - prev)
        if abs(delta) >= self.raw_jump_threshold_rad:
            since_last = now_sec - state['last_raw_jump_log_sec'][joint_index]
            if since_last >= self.raw_jump_log_min_interval_sec:
                self.get_logger().warning(
                    f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} RAW jump: prev_raw={prev:.6f} curr_raw={raw_sensor_angle:.6f} "
                    f"delta={delta:.6f} rad can_id=0x{msg.arbitration_id:X} data={msg.data.hex()}"
                )
                state['last_raw_jump_log_sec'][joint_index] = now_sec

        state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle

    def _should_reject_raw_jump(self, state, arm_name, joint_index: int, raw_sensor_angle: float, now_sec: float, msg: can.Message) -> bool:
        if raw_sensor_angle is None:
            return False

        # joint8 不做跳变丢弃过滤，仅更新基线用于后续比较
        if joint_index == 7:
            state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
            return False

        prev = state['prev_raw_sensor_angle'][joint_index]
        if prev is None:
            state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
            return False

        if not self.enable_raw_jump_reject:
            return False

        delta = self.wrap_to_pi(raw_sensor_angle - prev)
        if abs(delta) < self.raw_jump_reject_threshold_rad:
            return False

        since_last = now_sec - state['last_raw_reject_log_sec'][joint_index]
        raw_data = (msg.data[1] << 8) | msg.data[2]
        deg = (raw_data / 4096.0) * 360.0
        rad = math.radians(deg)
        rad = self.wrap_to_pi(rad)
        if since_last >= self.raw_jump_log_min_interval_sec:
            self.get_logger().warning(
                f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} RAW frame dropped: prev_raw={prev:.6f} curr_raw={raw_sensor_angle:.6f} "
                f"delta={delta:.6f} rad threshold={self.raw_jump_reject_threshold_rad:.6f} "
                f"can_id=0x{msg.arbitration_id:X} data={msg.data.hex()} "
                f"angle={rad:.6f} rad"
            )
            state['last_raw_reject_log_sec'][joint_index] = now_sec
        state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
        return True

    def extract_raw_sensor_angle_rad(self, data: bytes):
        if len(data) == 0:
            return None

        # 特殊格式：0xAA + 16bit角度，转换为 0~2pi
        if data[0] == 0xAA:
            if len(data) < 3:
                return None
            raw = (data[1] << 8) | data[2]
            deg = (raw / 4096.0) * 360.0
            rad = math.radians(deg)
            rad = self.wrap_to_pi(rad)
            return rad

        if len(data) < 2:
            return None

        # 通用格式：前2字节角度，默认 centi-rad
        # raw = int.from_bytes(data[0:2], byteorder='big', signed=True)
        # return self.raw_to_rad(raw)

    def wrap_to_pi(self, angle_rad: float) -> float:
        # 归一化到 [-pi, pi)，解决 0~2pi 到 ±pi 的过零映射问题
        return (angle_rad + math.pi) % (2.0 * math.pi) - math.pi

    def unwrap_joint_angle(self, joint_index: int, wrapped_angle: float, positions, has_position) -> float:
        # 基于上一帧做相位展开，避免在 -pi/+pi 边界发生数值跳变
        if not has_position[joint_index]:
            has_position[joint_index] = True
            return wrapped_angle

        prev = positions[joint_index]
        prev_wrapped = self.wrap_to_pi(prev)
        delta = wrapped_angle - prev_wrapped

        if delta > math.pi:
            delta -= 2.0 * math.pi
        elif delta < -math.pi:
            delta += 2.0 * math.pi

        return prev + delta

    def raw_to_rad(self, raw_value: int) -> float:
        if self.raw_angle_unit == 'centi-deg':
            return math.radians(raw_value / 100.0)
        # 默认按 centi-rad 处理，适配你当前 0~6.28 弧度输出
        return raw_value / 100.0

    def parse_sensor_from_can(self, data: bytes):
        if len(data) == 0:
            return 0.0, 0.0

        # 特殊格式：0xAA + 16bit角度 + 16bit速度
        if data[0] == 0xAA:
            if len(data) < 8:
                return 0.0, 0.0
            raw_deg = (data[1] << 8) | data[2]
            raw_vel = (data[3] << 8) | data[4]
            vel_deg = raw_vel / 100.0
            deg = (raw_deg / 4096.0) * 360.0
            rad = self.wrap_to_pi(math.radians(deg))
            vel_rad = math.radians(vel_deg)
            return rad, vel_rad

        if len(data) < 8:
            return 0.0, 0.0

        # # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析
        # raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True)
        # rad = self.wrap_to_pi(self.raw_to_rad(raw_deg))

        # raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True)
        # vel_rad = self.raw_to_rad(raw_vel)

        # return rad, vel_rad
        

def main():
    rclpy.init()
    node = JointSensorNode()
    rclpy.spin(node)
    node.shutdown()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()