代码-补充

1.

#!/usr/bin/env python

#!/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
from std_msgs.msg import String
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(30)  # 增加规划时间

        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

        # 将当前状态添加为第一个路径点
        waypoints.insert(0, current_pose)

        # 将所有四元数设置为默认值 (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)

        plan, fraction = self.group.compute_cartesian_path(
            waypoints,  # waypoints to follow
            0.05,       # eef_step
            0.2)        # jump_threshold

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

    def start(self):
        rospy.spin()

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

2.test_trajectory.py

#!/usr/bin/env python

import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
import geometry_msgs.msg
import sys
from copy import deepcopy

def main():
    roscpp_initialize(sys.argv)
    rospy.init_node('test_trajectory', anonymous=True)

    robot = RobotCommander()
    scene = PlanningSceneInterface()
    group = MoveGroupCommander("arm_group")
    group.set_planning_time(10)

    # 获取终端link的名称
    end_effector_link = group.get_end_effector_link()

    # 获取当前位姿数据作为机械臂运动的起始位姿
    start_pose = group.get_current_pose(end_effector_link).pose

    # 打印初始位姿
    rospy.loginfo("Start Pose:")
    rospy.loginfo("Position: x = %f, y = %f, z = %f", start_pose.position.x, start_pose.position.y, start_pose.position.z)
    rospy.loginfo("Orientation: x = %f, y = %f, z = %f, w = %f", start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w)

    # 设置终点
    end_pose = geometry_msgs.msg.Pose()
    end_pose.position.x = 0.21
    end_pose.position.y = -0.12
    end_pose.position.z = 0.24
    end_pose.orientation.x = start_pose.orientation.x
    end_pose.orientation.y = start_pose.orientation.y
    end_pose.orientation.z = start_pose.orientation.z
    end_pose.orientation.w = start_pose.orientation.w

    waypoints = [start_pose, end_pose]

    # 进行笛卡尔路径规划
    fraction = 0.0   # 路径规划覆盖率
    maxtries = 100   # 最大尝试规划次数
    attempts = 0     # 已经尝试规划次数

    # 设置机械臂当前的状态作为运动初始状态
    group.set_start_state_to_current_state()

    # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
    while fraction < 1.0 and attempts < maxtries:
        (plan, fraction) = group.compute_cartesian_path(
            waypoints,   # 路点列表
            0.01,        # eef_step，终端步进值
            0.0)         # jump_threshold，跳跃阈值

        # 尝试次数累加
        attempts += 1

        # 打印运动规划进程
        if attempts % 10 == 0:
            rospy.loginfo("Still trying after " + str(attempts) + " attempts...")

    # 如果路径规划成功（覆盖率100%），则开始控制机械臂运动
    if fraction == 1.0:
        rospy.loginfo("Path computed successfully. Moving the arm.")
        group.execute(plan, wait=True)
        rospy.loginfo("Path execution complete.")
    else:
        rospy.logwarn("Path planning failed with only %.2f%% success after %d attempts.", fraction * 100, attempts)

    roscpp_shutdown()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass