openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加坐标和姿态输出平滑

Browse Source
This commit is contained in:
shenchenyang 2026-03-19 14:17:49 +08:00
parent 049987f52d
commit a7b19d766b
1 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -279,13 +279,23 @@ class StereoHandDepthNode(Node):
# msg_out.pose.position.y = float(X) # msg_out.pose.position.y = float(X)
# msg_out.pose.position.z = -float(Y) # msg_out.pose.position.z = -float(Y)
# self.get_logger().info(f"X:{X:.3f}, Y:{Y:.3f}, Z:{Z:.3f}") # 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]) alpha = 0.2 # 平滑系数，0.0-1.0，越小越平滑
msg_out.pose.orientation.z = float(self.target_quat[2]) if not hasattr(self, 'smoothed_pos'):
msg_out.pose.orientation.w = float(self.target_quat[3]) self.smoothed_pos = self.target_pos.copy()
msg_out.pose.position.x = float(self.target_pos[0]) self.smoothed_quat = self.target_quat.copy()
msg_out.pose.position.y = float(self.target_pos[1]) else:
msg_out.pose.position.z = float(self.target_pos[2]) 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) self.pub.publish(msg_out)
# ----------- 可视化（调试用）----------- # ----------- 可视化（调试用）-----------