From 6934e6c7a310057c2206970c14ac0b778d8cbfe0 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Tue, 7 Apr 2026 18:18:02 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E5=BC=82=E5=B8=B8=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E6=8A=93=E5=8F=96=E5=92=8C=E8=BF=87=E6=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_joint_sensor/joint_control.py | 51 +++- .../openarm_joint_sensor/joint_sensor_can.py | 219 +++++++++++++++++- 2 files changed, 262 insertions(+), 8 deletions(-) diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py index 5fc001a..083ddcf 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py @@ -102,12 +102,20 @@ class ProgressActionClient(Node): self.declare_parameter('ema_alpha', 0.15) self.declare_parameter('kalman_q', 1e-5) self.declare_parameter('kalman_r', 1e-2) + self.declare_parameter('enable_jump_log', False) + self.declare_parameter('jump_velocity_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.enable_filter = self.get_parameter('enable_filter').get_parameter_value().bool_value self.filter_type = self.get_parameter('filter_type').get_parameter_value().string_value self.ema_alpha = self.get_parameter('ema_alpha').get_parameter_value().double_value self.kalman_q = self.get_parameter('kalman_q').get_parameter_value().double_value self.kalman_r = self.get_parameter('kalman_r').get_parameter_value().double_value + self.enable_jump_log = self.get_parameter('enable_jump_log').get_parameter_value().bool_value + self.jump_velocity_threshold_rad_s = self.get_parameter('jump_velocity_threshold_rad_s').get_parameter_value().double_value + self.jump_log_min_interval_sec = self.get_parameter('jump_log_min_interval_sec').get_parameter_value().double_value + self.joint_update_epsilon_rad = self.get_parameter('joint_update_epsilon_rad').get_parameter_value().double_value # Create per-joint filters self.num_joints = 8 @@ -116,9 +124,12 @@ class ProgressActionClient(Node): q=self.kalman_q, r=self.kalman_r) for _ in range(self.num_joints)] + self.prev_raw_positions = [None] * self.num_joints + self.last_update_time_sec = [None] * self.num_joints + self.last_jump_log_time_sec = [0.0] * self.num_joints # Buffer outgoing joint commands and send them at timer rate. - self.command_queue = deque(maxlen=50) + self.command_queue = deque(maxlen=20) self.subscriber_ = self.create_subscription( JointState, @@ -133,7 +144,7 @@ class ProgressActionClient(Node): # self._action_client = ActionClient(self, FollowJointTrajectory, '/right_joint_trajectory_controller/follow_joint_trajectory') # self.right_gripper_action_client = ActionClient(self, FollowJointTrajectory, '/right_joint_trajectory_controller/follow_joint_trajectory') - self.create_timer(0.02, self.timer_callback) # 定时器，周期为0.02秒 + self.create_timer(0.01, self.timer_callback) # 定时器，周期为0.02秒 # 3. 创建订阅者 # 参数1：消息类型（如String，与发布者一致） # 参数2：话题名称（如"chatter"，与发布者一致） @@ -150,6 +161,42 @@ class ProgressActionClient(Node): # self.get_logger().info(f'Received joint states: {list(msg.name)} positions: {list(msg.position)}') # Apply position filtering if enabled arm_pos = list(msg.position) + + # 基于“每关节速度阈值”检测异常，减少仅靠角度差导致的误报 + if self.enable_jump_log and len(arm_pos) >= self.num_joints: + now_sec = self.get_clock().now().nanoseconds * 1e-9 + for i in range(self.num_joints): + try: + curr = float(arm_pos[i]) + except Exception: + continue + + prev = self.prev_raw_positions[i] + if prev is None: + self.prev_raw_positions[i] = curr + self.last_update_time_sec[i] = now_sec + continue + + delta = curr - prev + if abs(delta) <= self.joint_update_epsilon_rad: + continue + + last_time = self.last_update_time_sec[i] + dt = 0.0 if last_time is None else (now_sec - last_time) + if dt > 1e-6: + speed = abs(delta) / dt + if speed >= self.jump_velocity_threshold_rad_s: + since_last_log = now_sec - self.last_jump_log_time_sec[i] + if since_last_log >= self.jump_log_min_interval_sec: + self.get_logger().warning( + f'Joint{i + 1} jump detected: prev={prev:.6f} curr={curr:.6f} ' + f'delta={delta:.6f} rad dt={dt:.4f} s speed={speed:.3f} rad/s' + ) + self.last_jump_log_time_sec[i] = now_sec + + self.prev_raw_positions[i] = curr + self.last_update_time_sec[i] = now_sec + if self.enable_filter: if len(arm_pos) >= self.num_joints: filtered = [] diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py index ca08eb6..0ad7c16 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py @@ -47,6 +47,12 @@ class JointSensorNode(Node): self.has_position = [False] * self.num_joints self.last_wrapped_positions = [0.0] * self.num_joints self.has_wrapped_position = [False] * self.num_joints + self.last_joint_update_sec = [None] * self.num_joints + self.last_jump_log_sec = [0.0] * self.num_joints + self.last_stage_log_sec = [0.0] * self.num_joints + self.prev_raw_sensor_angle = [None] * self.num_joints + self.last_raw_jump_log_sec = [0.0] * self.num_joints + self.last_raw_reject_log_sec = [0.0] * self.num_joints # 传感器原始单位：'centi-rad' 表示原始值/100后得到弧度；'centi-deg' 表示原始值/100后得到角度 self.raw_angle_unit = 'centi-rad' @@ -56,9 +62,31 @@ class JointSensorNode(Node): self.declare_parameter('sensor_directions', [1.0] * self.num_joints) self.declare_parameter('auto_zero_on_first_sample', False) self.declare_parameter('calibrate_now', False) + 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.35) self.zero_offsets = self._load_zero_offsets_from_param() self.sensor_directions = self._load_sensor_directions_from_param() self.auto_zero_on_first_sample = bool(self.get_parameter('auto_zero_on_first_sample').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.auto_zero_done = [False] * self.num_joints self.add_on_set_parameters_callback(self.on_set_parameters) @@ -75,21 +103,42 @@ class JointSensorNode(Node): 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(joint_index, raw_sensor_angle, msg_time_sec, msg): + return + + self._log_raw_jump(joint_index, raw_sensor_angle, msg_time_sec, msg) + # 每条CAN报文默认对应一个关节角和速度 - angle, vel = self.parse_sensor_from_can(msg.data) + parsed_angle, vel = self.parse_sensor_from_can(msg.data) direction = self.sensor_directions[joint_index] - angle = self.wrap_to_pi(angle * direction) + directed_angle = self.wrap_to_pi(parsed_angle * direction) vel = vel * direction - self.last_wrapped_positions[joint_index] = angle + self.last_wrapped_positions[joint_index] = directed_angle self.has_wrapped_position[joint_index] = True if self.auto_zero_on_first_sample and not self.auto_zero_done[joint_index]: - self.zero_offsets[joint_index] = angle + self.zero_offsets[joint_index] = directed_angle self.auto_zero_done[joint_index] = True - angle = self.wrap_to_pi(angle - self.zero_offsets[joint_index]) - angle = self.unwrap_joint_angle(joint_index, angle) + zeroed_angle = self.wrap_to_pi(directed_angle - self.zero_offsets[joint_index]) + angle = self.unwrap_joint_angle(joint_index, zeroed_angle) + + self._log_stage_diag( + joint_index, + parsed_angle, + directed_angle, + zeroed_angle, + angle, + vel, + msg_time_sec, + ) + self._log_speed_jump(joint_index, angle, msg_time_sec) + self.current_positions[joint_index] = angle self.current_velocities[joint_index] = vel @@ -168,6 +217,39 @@ class JointSensorNode(Node): if param.name == 'auto_zero_on_first_sample': self.auto_zero_on_first_sample = 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 == 'calibrate_now' and bool(param.value): self.calibrate_zero_from_current() @@ -182,6 +264,131 @@ class JointSensorNode(Node): return SetParametersResult(successful=True) + def _log_stage_diag( + self, + 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 - self.last_stage_log_sec[joint_index] + if since_last < self.stage_log_min_interval_sec: + return + + self.last_stage_log_sec[joint_index] = now_sec + self.get_logger().info( + f"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, joint_index: int, curr_unwrapped_angle: float, now_sec: float): + if not self.enable_speed_jump_log: + self.last_joint_update_sec[joint_index] = now_sec + return + + prev_time = self.last_joint_update_sec[joint_index] + if prev_time is None: + self.last_joint_update_sec[joint_index] = now_sec + return + + dt = now_sec - prev_time + if dt <= 1e-6: + self.last_joint_update_sec[joint_index] = now_sec + return + + prev_unwrapped = self.current_positions[joint_index] + delta = curr_unwrapped_angle - prev_unwrapped + if abs(delta) <= self.joint_update_epsilon_rad: + self.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 - self.last_jump_log_sec[joint_index] + if since_last_log >= self.jump_log_min_interval_sec: + self.get_logger().warning( + f"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" + ) + self.last_jump_log_sec[joint_index] = now_sec + + self.last_joint_update_sec[joint_index] = now_sec + + def _log_raw_jump(self, 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: + self.prev_raw_sensor_angle[joint_index] = raw_sensor_angle + return + + prev = self.prev_raw_sensor_angle[joint_index] + if prev is None: + self.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 - self.last_raw_jump_log_sec[joint_index] + if since_last >= self.raw_jump_log_min_interval_sec: + self.get_logger().warning( + f"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()}" + ) + self.last_raw_jump_log_sec[joint_index] = now_sec + + self.prev_raw_sensor_angle[joint_index] = raw_sensor_angle + + def _should_reject_raw_jump(self, joint_index: int, raw_sensor_angle: float, now_sec: float, msg: can.Message) -> bool: + if raw_sensor_angle is None: + return False + + prev = self.prev_raw_sensor_angle[joint_index] + if prev is None: + self.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 - self.last_raw_reject_log_sec[joint_index] + if since_last >= self.raw_jump_log_min_interval_sec: + self.get_logger().warning( + f"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()}" + ) + self.last_raw_reject_log_sec[joint_index] = now_sec + + 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] + return (raw / 4096.0) * (2.0 * math.pi) + + 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