完善代码：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseArray, Pose
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
import sys
import threading

class TrajectoryReceiver:
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('trajectory_receiver', anonymous=True)

        self.robot = RobotCommander()
        self.scene = PlanningSceneInterface()
        self.group = MoveGroupCommander("arm_group")
        self.group.set_planning_time(60)  # 增加规划时间
        self.group.set_max_velocity_scaling_factor(0.5)  # 减小速度，提高成功率
        self.group.set_max_acceleration_scaling_factor(0.5)  # 减小加速度，提高成功率

        self.trajectory_subscriber = rospy.Subscriber("robot_arm_trajectory", PoseArray, self.trajectory_callback)
        self.waypoints = []
        self.lock = threading.Lock()

    def trajectory_callback(self, msg):
        rospy.loginfo("Received trajectory with %d points", len(msg.poses))
        with self.lock:
            self.waypoints = list(msg.poses)
        threading.Thread(target=self.process_trajectory).start()

    def process_trajectory(self):
        with self.lock:
            waypoints = self.waypoints

        if not waypoints:
            return

        rospy.loginfo("Processing trajectory with %d points", len(waypoints))

        # 获取机械臂的当前状态
        current_pose = self.group.get_current_pose().pose

        # 移动机械臂到第一个轨迹点
        first_point = waypoints[0]
        self.group.set_pose_target(first_point)
        self.group.go(wait=True)
        self.group.stop()
        self.group.clear_pose_targets()

        # 将所有四元数设置为固定值 (0, 0, 0, 1)
        for i in range(len(waypoints)):
            waypoints[i].orientation.x = 0.0
            waypoints[i].orientation.y = 0.0
            waypoints[i].orientation.z = 0.0
            waypoints[i].orientation.w = 1.0

            # 打印每个路径点的信息
            rospy.loginfo("Waypoint %d: x=%f, y=%f, z=%f, qx=%f, qy=%f, qz=%f, qw=%f",
                          i, waypoints[i].position.x, waypoints[i].position.y, waypoints[i].position.z,
                          waypoints[i].orientation.x, waypoints[i].orientation.y, waypoints[i].orientation.z, waypoints[i].orientation.w)

        # 分段规划和执行
        num_segments = 5  # 将路径分为5段
        segment_length = len(waypoints) // num_segments

        success = True
        for i in range(num_segments):
            start_idx = i * segment_length
            end_idx = (i + 1) * segment_length if i < num_segments - 1 else len(waypoints)

            segment = waypoints[start_idx:end_idx]
            plan, fraction = self.group.compute_cartesian_path(
                segment,  # waypoints to follow
                0.01,     # eef_step
                0.0)      # jump_threshold

            if fraction == 1.0:
                rospy.loginfo("Segment %d planned successfully. Moving the arm.", i)
                self.group.execute(plan, wait=True)
            else:
                rospy.logwarn("Segment %d planning failed with only %.2f%% success.", i, fraction * 100)
                success = False
                break

        if success:
            rospy.loginfo("All segments planned and executed successfully.")
        else:
            rospy.logwarn("Path planning failed.")

    def start(self):
        rospy.spin()

if __name__ == "__main__":
    try:
        traj_receiver = TrajectoryReceiver()
        traj_receiver.start()
    except rospy.ROSInterruptException:
        pass
    finally:
        roscpp_shutdown()