sajdldnias

import sys
import time
import numpy as np
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
import moveit_commander
from geometry_msgs.msg import PoseStamped

# 初始化 moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_realtime_monitor', anonymous=True)

# 设置机器人和运动组
robot = RobotCommander()
scene = PlanningSceneInterface()
group = MoveGroupCommander("arm_group")  # 修改为 MyCobot 的规划组名称

# 计算轨迹路径长度（关节空间距离）
def calculate_path_length(trajectory):
    if not trajectory or not trajectory.joint_trajectory.points:
        return 0.0  # 如果轨迹为空，返回0长度
    
    length = 0.0
    points = trajectory.joint_trajectory.points

    # 遍历轨迹中的每个点，计算相邻点之间的欧几里得距离
    for i in range(1, len(points)):
        point1 = np.array(points[i - 1].positions)
        point2 = np.array(points[i].positions)
        dist = np.linalg.norm(point2 - point1)
        length += dist

    return length

# 实时监控并执行任务
while not rospy.is_shutdown():
    # 设置目标位姿（每次随机或从列表中选定目标点）
    target_pose = PoseStamped()
    target_pose.header.frame_id = "base_link"  # 基座坐标系
    target_pose.pose.position.x = 0.15         # 目标位置的 X 坐标
    target_pose.pose.position.y = 0.0          # 目标位置的 Y 坐标
    target_pose.pose.position.z = 0.2          # 目标位置的 Z 坐标
    target_pose.pose.orientation.w = 1.0       # 设置方向
    group.set_pose_target(target_pose)

    # 记录轨迹规划开始时间
    start_time = time.time()

    # 执行轨迹规划
    plan = group.plan()

    # 记录轨迹规划结束时间
    end_time = time.time()

    # 输出轨迹规划的时间
    planning_time = end_time - start_time

    # 如果计划成功，计算路径长度
    if plan and plan.joint_trajectory.points:
        path_length = calculate_path_length(plan)
        print(f"轨迹规划完成！")
        print(f"轨迹规划时间：{planning_time:.4f} 秒")
        print(f"轨迹路径长度：{path_length:.4f} 单位长度")
    else:
        print("轨迹规划失败，无法计算路径长度")

    # 执行规划的轨迹
    execution_start = time.time()
    execution_result = group.execute(plan, wait=True)
    execution_end = time.time()

    if execution_result:
        execution_time = execution_end - execution_start
        print(f"轨迹执行完成！执行时间：{execution_time:.4f} 秒")
    else:
        print("轨迹执行失败！")

    # 等待一段时间后重复下一次任务（可调整时间）
    rospy.sleep(2.0)

# 关闭 moveit_commander
moveit_commander.roscpp_shutdown()