ROS/lstplanning_dev/src/lstrobot_planning/scripts/moveitServer2.py

# 导入基本ros和moveit库
from logging.handlers import RotatingFileHandler
import os
import moveit_msgs.msg
import rospy, sys
import moveit_commander      
import moveit_msgs
from visualization_msgs.msg import Marker, MarkerArray

from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
from moveit_msgs.msg import  PlanningScene, ObjectColor,CollisionObject, AttachedCollisionObject,RobotTrajectory
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
from shape_msgs.msg import SolidPrimitive
from sensor_msgs.msg import JointState
import tf.transformations
from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose,Point,Vector3

from copy import deepcopy
import numpy as np
import tf
from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
import math
import traceback
from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
import json
import termios
from decorator_time import decorator_time
import check
from redis_publisher import Publisher

pi = np.pi

class MoveIt_Control:
    # TODO初始化程序
    def __init__(self, is_use_gripper=False):
        self.home_pose = []
        self.hf_num = 0
        self.hf_fail = []
        
        # Init ros config
        moveit_commander.roscpp_initialize(sys.argv)
        self.robot = moveit_commander.RobotCommander()  #暂未使用
        
        self.arm = moveit_commander.MoveGroupCommander('manipulator')
        self.arm.set_goal_joint_tolerance(0.0001)
        self.arm.set_goal_position_tolerance(0.0005)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.end_effector_link = self.arm.get_end_effector_link()
        # 设置机械臂基座的参考系
        self.reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(self.reference_frame)

        # # 设置最大规划时间和是否允许重新规划
        self.arm.set_planning_time(10.0)
        self.arm.allow_replanning(True)

        self.arm.set_max_acceleration_scaling_factor(1)
        self.arm.set_max_velocity_scaling_factor(1)

        self.arm.set_num_planning_attempts(10)
        # 机械臂初始姿态
        #self.move_j()	  #备注，是否可以用move_j代替go_home来节省时间(待测试)
        self.go_home()
        self.home_pose = self.get_now_pose()

    # TODO关节规划，输入6个关节角度（单位：弧度）
    def move_j(self, joint_configuration=None, a=1, v=1):
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        if joint_configuration == None:
            joint_configuration = [0, 0, 0, 0, 0, 0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        self.arm.set_joint_value_target(joint_configuration)
        rospy.loginfo("move_j:" + str(joint_configuration))
        self.arm.go()
        #rospy.sleep(1)
        
    # TODO获取机械臂当前位姿
    def get_now_pose(self):
        current_pose = self.arm.get_current_pose(self.end_effector_link).pose
        pose = []
        pose.append(current_pose.position.x)
        pose.append(current_pose.position.y)
        pose.append(current_pose.position.z)

        pose.append(current_pose.orientation.x)
        pose.append(current_pose.orientation.y)
        pose.append(current_pose.orientation.z)
        pose.append(current_pose.orientation.w)
        # 打印位姿信息       
        #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
        return pose
    
    #TODO move_p 路径约束   
    def create_path_constraints2(self, start_point, end_point, r):
        #计算起点指向终点的向量
        vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
        height = np.linalg.norm(vector) + 0.05  #高度延长16cm
        radius = r
        
        # 位置约束
        position_constraint = PositionConstraint()          #实例化位置约束
        position_constraint.header.frame_id = "base_link"   #此约束所指的机器人链接
        position_constraint.link_name = self.end_effector_link
        position_constraint.target_point_offset = Vector3(0, 0, 0)  #目标点的偏移量
        position_constraint.weight = 1.0   #权重，'1'为硬约束

        #构建 shape_msgs/SolidPrimitive 消息
        bounding_volume = SolidPrimitive()
        bounding_volume.type = SolidPrimitive.CYLINDER   #圆柱约束
        bounding_volume.dimensions = [height, radius]    #圆柱的尺寸，[高度，半径]
        position_constraint.constraint_region.primitives.append(bounding_volume)

        #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
        pose = Pose()
        pose.position.x = start_point[0] + vector[0] / 2    #圆柱的几何中心
        pose.position.y = start_point[1] + vector[1] / 2 
        pose.position.z = start_point[2] + vector[2] / 2
        
        # 计算圆柱体的姿态
        z_axis = np.array([0, 0, 1])
        axis = np.cross(z_axis, vector) #计算两个向量（向量数组）的叉乘  叉乘返回的数组既垂直于a，又垂直于b
        angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))  #反余弦求角度
        q = tf.transformations.quaternion_about_axis(angle, axis)           #通过旋转轴和旋转角返回四元数
        pose.orientation.x = q[0]
        pose.orientation.y = q[1]
        pose.orientation.z = q[2]
        pose.orientation.w = q[3]
        position_constraint.constraint_region.primitive_poses.append(pose)

        constraints = Constraints() #创建一个新的约束信息 
        constraints.position_constraints.append(position_constraint)
        
        return constraints
    
    #TODO move_p_flexible 转点规划
    def move_p_flexible(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        rrr = 0.2 #初始允许半径 20cm, 点到点转焊缝，可适当放宽初始半径
        er = 0
        
        if trajectory:
            #起点位置设置为规划组最后一个点，即上一次移动的终点
            state = self.arm.get_current_state()
            state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
            path_constraints = self.create_path_constraints2(points[-1], point, rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
        else:
            #刚开始规划 起点位置设定为当前状态  按理来说是home点 
            self.go_home()
            self.home_pose = self.get_now_pose()
            state = self.arm.get_current_state()
            path_constraints = self.create_path_constraints2(self.home_pose, point, rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束

        self.arm.set_path_constraints(path_constraints)
        self.arm.set_pose_target(point, self.end_effector_link)
        self.arm.set_start_state(state) #起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
        
        #尝试规划次数共10次，带约束规划5次仍失败取消约束重新规划5次
        b = 10
        for i in range(b):
            traj = self.arm.plan()  #调用plan进行规划(OMPL)
            error = traj[3]
            if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
                trajj = traj[1] #取出规划的信息
                rospy.loginfo("规划完成! 正在检查移动轨迹有效性...")
                error_c, limit_margin = check.check_joint_positions(trajj)
                if not error_c:       
                    rospy.loginfo("本次移动OK")
                    rospy.loginfo("*******************")
                    trajj_with_type = mark_the_traj(trajj,"during-p", welding_sequence)     
                    trajectory_with_type.append(trajj_with_type)
                    points.append(point)
                    trajectory.append(trajj)  
                    break
                else:
                    rospy.loginfo("check failed! 移动轨迹无效")
                    rospy.loginfo("移动轨迹检查失败-开始第{}次重新规划".format(i+1))
                    self.arm.clear_path_constraints()
                    rrr += 0.2 #每次增加20厘米半径
                    rospy.loginfo("R值: {}".format(rrr))
                    if trajectory:
                        path_constraints = self.create_path_constraints2(points[-1], point, rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
                    else:
                        path_constraints = self.create_path_constraints2(self.home_pose, point, rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
                    self.arm.set_path_constraints(path_constraints)
                    
                    if(i >= (b-5)):
                        rospy.loginfo("约束移动规划尝试失败,取消约束重新规划")
                        self.arm.clear_path_constraints()
                        
                    if(i == (b-1)):
                        rospy.loginfo("所有移动规划尝试失败,焊缝起点不可达!")
                        er = 1
                        break  
            else:
                er = 1
                rospy.loginfo("移动规划失败，焊缝起点不可达！") 
                break
            
        #清除约束
        self.arm.clear_path_constraints()
        
        return points, trajectory, trajectory_with_type, er
    
    #TODO move_pl 路径约束
    def create_path_constraints(self, start_point, end_point):
        #计算起点指向终点的向量
        vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
        height = np.linalg.norm(vector) + 0.002
        radius = 0.01

        constraints = Constraints()
        
        # 位置约束
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = "base_link"
        position_constraint.link_name = self.end_effector_link
        position_constraint.target_point_offset = Vector3(0, 0, 0)
        #构建 shape_msgs/SolidPrimitive 消息
        bounding_volume = SolidPrimitive()
        bounding_volume.type = SolidPrimitive.CYLINDER
        # bounding_volume.dimensions = [0.003,1]
        bounding_volume.dimensions = [height, radius]
        #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
        pose = Pose()
        pose.position.x = start_point[0] + vector[0] / 2
        pose.position.y = start_point[1] + vector[1] / 2 
        pose.position.z = start_point[2] + vector[2] / 2
        
        # 计算圆柱体的旋转矩阵
        z_axis = np.array([0, 0, 1])
        axis = np.cross(z_axis, vector)
        angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))
        q = tf.transformations.quaternion_about_axis(angle, axis)
        pose.orientation.x = q[0]
        pose.orientation.y = q[1]
        pose.orientation.z = q[2]
        pose.orientation.w = q[3]

        position_constraint.constraint_region.primitives.append(bounding_volume)
        position_constraint.constraint_region.primitive_poses.append(pose)
        position_constraint.weight = 1.0
        
        # 角度约束
        # target_pose = Pose()
        # target_pose.orientation.x = start_point[3]
        # target_pose.orientation.y = start_point[4]
        # target_pose.orientation.z = start_point[5]
        # target_pose.orientation.w = start_point[6]
        
        # Orientation_Constraint = OrientationConstraint()
        # Orientation_Constraint.link_name = self.end_effector_link
        # Orientation_Constraint.header.frame_id = "base_link"
        # Orientation_Constraint.orientation = target_pose.orientation
        # Orientation_Constraint.absolute_x_axis_tolerance = 0.01
        # Orientation_Constraint.absolute_y_axis_tolerance = 0.01
        # Orientation_Constraint.absolute_z_axis_tolerance = 0.01
        # Orientation_Constraint.weight = 1.0  # 权重，硬约束

        constraints.position_constraints.append(position_constraint)
        # constraints.orientation_constraints.append(Orientation_Constraint)

        return constraints

    #TODO move_pl 焊缝规划，规划当前点(焊缝起点)和焊缝终点之间的路径
    def move_pl(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
        
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)

        #move_p规划成功，则move_p规划路径中的最后一个关节轨迹点为move_pl的起点
        if trajectory:
            state = self.arm.get_current_state()
            #路径中的最后一个关节轨迹点
            state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
            self.arm.set_start_state(state)
            
        else:
            """
            第一次规划超时(Time_out),但并未返回Error_code;此时move_p并不会返回任何路径点信息
            到points列表中,此时机器人应还处在home点位置,在此将home点直接添加到points列表中,与
            焊缝终点做路径约束,并规划路径 (虽然不再报错继续执行，但该条焊缝仍规划失败)
            """
            points.append(self.home_pose)       
                
        self.arm.set_pose_target(point, self.end_effector_link) #设定目标点为焊缝终点
                
        #创建路径约束
        path_constraints = self.create_path_constraints(points[-1], point)   #将最后一个点(应为起始点)和输入的点(焊缝的起始点)做圆柱轨迹约束
        self.arm.set_path_constraints(path_constraints)#设定约束

        traj = self.arm.plan()
        error = traj[3]
        if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
            rospy.loginfo("本次焊缝规划 OK")
            rospy.loginfo("*******************")
            trajj = traj[1] #取出规划的信息
            trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
            trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
            trajj_with_type.points[-1].type = "end"
            points.append(point)
            trajectory.append(trajj)
            trajectory_with_type.append(trajj_with_type)
        else:
            rospy.loginfo("本次焊缝规划失败")
            points.pop()    #将本条焊缝的起点从规划中删除
            # 同样第一次move_p提示timeout, trajectory中同样不会有路径信息
            if trajectory:
                trajectory.pop()    
                trajectory_with_type.pop()
                
            self.hf_fail.append(welding_sequence[self.hf_num])  #将焊缝添加到失败列表

        self.arm.clear_path_constraints()
        self.hf_num = self.hf_num + 1   #焊缝计数
        return points, trajectory, trajectory_with_type

    # 暂未使用
    def move_pl_check(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        
        #重新规划起点次数
        a=10
        for j in range(a):
            if trajectory:
                state = self.arm.get_current_state()
                state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
                self.arm.set_start_state(state)
                
            self.arm.set_pose_target(point, self.end_effector_link)
            
            path_constraints = self.create_path_constraints(points[-1], point)
            self.arm.set_path_constraints(path_constraints)
            
            #当前起点规划次数
            b=10
            for i in range(b):
                traj = self.arm.plan()
                error = traj[3]
                if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
                    trajj = traj[1] #取出规划的信息
                    rospy.loginfo("正在检查焊缝轨迹有效性...")
                    error_c,limit_margin = check.check_joint_positions(trajj)
                    if not error_c:       
                        rospy.loginfo("本次焊缝规划OK")
                        rospy.loginfo("*******************")
                        break
                    if not limit_margin or i == b-1:
                        prepoint = points.pop()
                        trajectory.pop()
                        trajectory_with_type.pop()
                        #清除约束
                        self.arm.clear_path_constraints()
                        points, trajectory, trajectory_with_type, er1 = self.move_p_flexible(prepoint, points, trajectory, trajectory_with_type)
                        break
                rospy.loginfo("The {}-{}th replanning".format(j,i+1))
            if not error_c:
                break
        if error_c:
            rospy.loginfo("本次焊缝规划失败")
            rospy.loginfo("*******************")
            self.hf_fail.append(welding_sequence[self.hf_num])
            
        self.arm.clear_path_constraints()
        self.hf_num = self.hf_num + 1#焊缝计数
        
        trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
        trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
        trajj_with_type.points[-1].type = "end"      

        points.append(point)
        trajectory.append(trajj)
        trajectory_with_type.append(trajj_with_type)
        
        return points, trajectory, trajectory_with_type
     
    #TODO go_home
    @decorator_time  
    def go_home(self, a=1, v=1):
        rospy.loginfo("go_home start")
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        self.arm.set_named_target('home')
        rospy.loginfo("get_home end")
        self.arm.go()
        rospy.loginfo("go_home end")
        # rospy.sleep(1)
    
    #TODO go_home_justplan
    def go_home_justplan(self, trajectory, trajectory_with_type, a=1, v=1):
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        state = self.arm.get_current_state()
        state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
        self.arm.set_start_state(state)
        self.arm.set_named_target('home')
        traj = self.arm.plan()
        trajj = traj[1]
        traj_with_type = mark_the_traj(trajj, "go-home", welding_sequence)
        trajectory.append(trajj)
        trajectory_with_type.append(traj_with_type)
        return trajectory, trajectory_with_type

    #TODO path_planning
    @decorator_time
    def path_planning(self, folder_path, gohome=True):
        file_path_result = os.path.join(folder_path, 'result.txt')
        all_data = process_welding_data(file_path_result)
        err = 0
        points, trajectory, trajectory_with_type = [],[],[]
        for i in range(len(all_data)):
            rospy.loginfo("共读取到%d条焊缝，开始规划第%d条", len(all_data), i+1)
            start_point = all_data[i][0]
            end_point = all_data[i][1]
            q1 = all_data[i][2]
            q2 = all_data[i][3]

            point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0], q1[1], q1[2], q1[3]]
            points, trajectory, trajectory_with_type, err = self.move_p_flexible(point11, points, trajectory, trajectory_with_type)
            # 焊缝起点不可达 则跳过此次焊缝和移动的规划 并将焊缝添加到失败列表 
            if err == 1:      
                self.hf_fail.append(welding_sequence[self.hf_num])
                self.hf_num = self.hf_num+1 # 焊缝计数
                continue

            point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0], q2[1], q2[2], q2[3]]
            points, trajectory, trajectory_with_type = self.move_pl(point22, points, trajectory, trajectory_with_type, v=speed_v)

            rospy.loginfo("第%d条焊缝规划完毕", i+1)
            rospy.loginfo("*******************")
        if gohome:
            points, trajectory, trajectory_with_type, err = self.move_p_flexible(self.home_pose, points, trajectory, trajectory_with_type, v=speed_v)
            trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
        traj_merge = merge_trajectories(trajectory)
        trajectory_with_type_merge = merge_trajectories_with_type(trajectory_with_type)
        rospy.loginfo("All of The trajectory Plan successfully")
        return trajectory, traj_merge, trajectory_with_type_merge

###########################################################class end#############################################################
def process_welding_data(filename):
    all_data = []  # 用来存储每一行处理后的数据
    with open(filename, 'r') as file:
        for line in file:
            parts = line.strip().split('/')
            coordinates_and_poses = [part.split(',') for part in parts[1:]]
            
            start_point = tuple(map(float, coordinates_and_poses[0][:3]))  # 使用元组以避免修改
            end_point = tuple(map(float, coordinates_and_poses[1][:3]))
            q1 = tuple(map(float, coordinates_and_poses[2][:4]))
            q2 = tuple(map(float, coordinates_and_poses[3][:4]))
            
            # 收集每行处理后的数据
            all_data.append((start_point, end_point, q1, q2))

    return all_data

#TODO mark_the_traj 标记轨迹类型(during-p, during-l, go-home)
def mark_the_traj(traj, TYPE, SEQUENCE):
    traj_with_type = JointTrajectory_ex()
    traj_with_type.header = traj.joint_trajectory.header
    traj_with_type.joint_names = traj.joint_trajectory.joint_names
    traj_with_type.points = [
        JointTrajectoryPoint_ex(
            positions = point.positions,
            velocities = point.velocities,
            accelerations = point.accelerations,
            effort = point.effort,
            type = TYPE,
            sequence = SEQUENCE
        ) for point in traj.joint_trajectory.points
    ]
    return traj_with_type

#TODO merge_trajectories 合并所有关节轨迹点
def merge_trajectories(trajectories):
    if not trajectories:
        return None
    
    # 创建一个新的 JointTrajectory 对象
    merged_trajectory = JointTrajectory()
    merged_trajectory.header = trajectories[0].joint_trajectory.header
    merged_trajectory.joint_names = trajectories[0].joint_trajectory.joint_names
    
    # 初始化时间累加器
    last_time_from_start = rospy.Duration(0)
    
    # 合并所有 trajectories 的 points
    for traj in trajectories:

        for point in traj.joint_trajectory.points:
            # 创建新的轨迹点
            new_point = deepcopy(point)
            # 累加时间
            new_point.time_from_start += last_time_from_start
            merged_trajectory.points.append(new_point)
        
        # 更新时间累加器为当前轨迹的最后一个点的时间
        if traj.joint_trajectory.points:
            last_time_from_start = traj.joint_trajectory.points[-1].time_from_start + last_time_from_start
    
    return merged_trajectory

#TODO merge_trajectories_with_type 合并所有带焊缝起始点标记的关节轨迹点
def merge_trajectories_with_type(trajectory_with_type):
    if not trajectory_with_type:
        return None
    
    # 创建一个新的 JointTrajectory 对象
    merged_trajectory_with_type = JointTrajectory_ex()
    merged_trajectory_with_type.header = trajectory_with_type[0].header
    merged_trajectory_with_type.joint_names = trajectory_with_type[0].joint_names
    
    # 初始化时间累加器
    last_time_from_start = rospy.Duration(0)
    
    # 合并所有 trajectories 的 points
    for traj in trajectory_with_type:

        for point in traj.points:
            # 创建新的轨迹点
            new_point = deepcopy(point)
            # 累加时间
            new_point.time_from_start += last_time_from_start
            merged_trajectory_with_type.points.append(new_point)
        
        # 更新时间累加器为当前轨迹的最后一个点的时间
        if traj.points:
            last_time_from_start = traj.points[-1].time_from_start + last_time_from_start
    
    return merged_trajectory_with_type

def pub_trajectories(trajectory_with_type_merge):
   
    pub = rospy.Publisher("/joint_path", JointTrajectory_ex, queue_size=5)
    count = 1
    rate = rospy.Rate(1)
    rospy.loginfo("正在发布轨迹信息..")
    while not rospy.is_shutdown():
        #轨迹接受完毕，关闭当前发布
        sign_traj_accepted = str(rospy.get_param("sign_traj_accepted"))
        if sign_traj_accepted == "1":
            rospy.set_param("sign_traj_accepted",0)
            break

        pub.publish(trajectory_with_type_merge)
        # rospy.loginfo("发布计数,count = %d",count)
        # count += 1
        rate.sleep()


class py_msgs():
    fial = []
    shun_xv = []
    class point():
        xyz_list = []
        type = []

# ROS2PY_msgs 将规划后的轨迹信息打包成json格式发送到redis
def ROS2PY_msgs(msgs, m_sr):
    for i in range(len(msgs.points)):
        py_msgs.point.xyz_list.append(msgs.points[i].positions)
        py_msgs.point.type.append(msgs.points[i].type)
    py_msgs.fail = m_sr.hf_fail.copy()
    py_msgs.shun_xv = msgs.points[0].sequence.copy()

    # for i in range(len(py_msgs.point.type)):
    #     print(py_msgs.point.xyz_list[i])
    #     print(py_msgs.point.type[i])

    message = {
        'positions': py_msgs.point.xyz_list,  # 规划路径点结果
        'flags': py_msgs.point.type,          # 规划路径点的类型，焊接点移动点
        'weld_order': py_msgs.shun_xv,        # 规划路径顺序
        'failed': py_msgs.fail,               # 规划路径失败的焊缝
    }
    return json.dumps(message)
    # for i in range(len(py_msgs.point.type)):
    #     moveit_server.move_j(py_msgs.point.xyz_list[i])
    #     #input("任意键继续")

#TODO get_valid_input 监听用户输入
def get_valid_input(factor, default):
    while True:
        user_input = input(factor)
        if user_input == "":
            return default
        try:
            value = float(user_input)
            if 0 < value <= 1:
                return value
            else:
                rospy.logerr("缩放因子必须在(0, 1]范围内，请重新输入!")
        except ValueError:
            rospy.logerr("输入值无效，请根据提示重新输入！")

# TODO get_uesr_input 获取用户输入
def get_user_input():
    """获取用户输入的速度和加速度缩放因子，并在控制台中打印信息和错误提示"""
    DEFUALT_V = 1.0
    DEFUALT_A = 1.0
     
    try:
        termios.tcflush(sys.stdin, termios.TCIFLUSH)   #清空输入缓存
        rospy.loginfo("输入缓存区已清空！")
        
        vv = get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
        #a=get_valid_input("请输入加速度缩放因子(0-1, 默认为1): ", DEFUALT_A)
          
        #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
        return vv
    except Exception as e:
        rospy.logerr(f"发生错误：{e}")
        
def ROS2_msgs(msgs, m_sr):
    for i in range(len(msgs.points)):
        py_msgs.point.xyz_list.append(msgs.points[i].positions)
    
    f_p = os.path.join(folder_path, 'outtt.txt')
    with open(f_p, 'w') as file:
        for point in py_msgs.point.xyz_list:
            file.write(' '.join(str(value) for value in point) + "\n")

if __name__ =="__main__":

    folder_path = rospy.get_param("folder_path")
    rospy.init_node('moveit_control_server', anonymous=False)   
    moveit_server = MoveIt_Control(is_use_gripper=False)  
    welding_sequence = rospy.get_param('welding_sequence')

    speed_v = 1.0
    # speed_v=get_user_input()
    # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")

    trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
    #ROS2_msgs(trajectory_with_type_merge,moveit_server)
    #write_ee(trajectory_with_type_merge, moveit_server)
    
    moveit_server.arm.execute(trajectory_merge)
    print(f"本次规划失败焊缝序号:{moveit_server.hf_fail}")
    
    #使用redis发布规划后的轨迹信息
    #message = ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
    #publisher = Publisher()
    #publisher.pub_plan_result(message)
    
    rospy.set_param("sign_control",0)