From a7b19d766bbda6ed74c3dc0db963b16f88cb8475 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Thu, 19 Mar 2026 14:17:49 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=9D=90=E6=A0=87=E5=92=8C?= =?UTF-8?q?=E5=A7=BF=E6=80=81=E8=BE=93=E5=87=BA=E5=B9=B3=E6=BB=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_camera/stereo_hand_depth_node.py | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py index 271f1a0..004a3d5 100644 --- a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py +++ b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py @@ -279,13 +279,23 @@ class StereoHandDepthNode(Node): # msg_out.pose.position.y = float(X) # msg_out.pose.position.z = -float(Y) # self.get_logger().info(f"X:{X:.3f}, Y:{Y:.3f}, Z:{Z:.3f}") - msg_out.pose.orientation.x = float(self.target_quat[0]) - msg_out.pose.orientation.y = float(self.target_quat[1]) - msg_out.pose.orientation.z = float(self.target_quat[2]) - msg_out.pose.orientation.w = float(self.target_quat[3]) - msg_out.pose.position.x = float(self.target_pos[0]) - msg_out.pose.position.y = float(self.target_pos[1]) - msg_out.pose.position.z = float(self.target_pos[2]) + # 对位置和姿态做简单平滑（滑动平均） + alpha = 0.2 # 平滑系数，0.0-1.0，越小越平滑 + if not hasattr(self, 'smoothed_pos'): + self.smoothed_pos = self.target_pos.copy() + self.smoothed_quat = self.target_quat.copy() + else: + self.smoothed_pos = alpha * self.target_pos + (1 - alpha) * self.smoothed_pos + self.smoothed_quat = alpha * self.target_quat + (1 - alpha) * self.smoothed_quat + self.smoothed_quat = self.smoothed_quat / np.linalg.norm(self.smoothed_quat) # 归一化 + + msg_out.pose.orientation.x = float(self.smoothed_quat[0]) + msg_out.pose.orientation.y = float(self.smoothed_quat[1]) + msg_out.pose.orientation.z = float(self.smoothed_quat[2]) + msg_out.pose.orientation.w = float(self.smoothed_quat[3]) + msg_out.pose.position.x = float(self.smoothed_pos[0]) + msg_out.pose.position.y = float(self.smoothed_pos[1]) + msg_out.pose.position.z = float(self.smoothed_pos[2]) self.pub.publish(msg_out) # ----------- 可视化（调试用）-----------