更新2024-8-6

catkin_ws/src/lstrobot_planning/.vscode/c_cpp_properties.json

@@ -0,0 +1,22 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/robot/ROS/catkin_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/robot/ROS/COMP_URDF/src/moveit_demo/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

catkin_ws/src/lstrobot_planning/scripts/11.py

@@ -4,20 +4,12 @@ import os
 from std_srvs.srv import Empty
 from subprocess import Popen
 
-def Start_up_test():
-    # 设置各种类型参数
-    
-    # rospy.set_param("folder_path","/home/chen/catkin_ws/data/20240703-11-法兰盘")
-    rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/20240710-2")
-    # rospy.set_param("culling_radius",10)
-    rospy.set_param("sign_control",1)
-    # rospy.set_param("sign_control",2)
+
 if __name__ == '__main__':
     
     rospy.init_node("test")
-    
+    rospy.set_param("sign_control",1)
+   
    
-    Start_up_test()
-
 
      

catkin_ws/src/lstrobot_planning/scripts/check.py

@@ -1,148 +0,0 @@
-import rospy, sys
-import numpy as np
-from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
-
-# def traj_validity_check(trajectory):
-#     if type (trajectory)is moveit_msgs.msg._RobotTrajectory.RobotTrajectory:
-#         point_num=len(trajectory.joint_trajectory.point)
-#         trajectory.joint_trajectory.point.positions
-        
-#         # for i
-#         trajectory.joint_trajectory.point.velocities
-
-#         trajectory.joint_trajectory.point.accelerations
-        
-#         return 0
-#     # else:
-#     #         raise MoveItCommanderException(
-#     #             "Expected value in the range from 0 to 1 for scaling factor"
-#     #         )
-#     def scale_trajectory_speed(traj, scale):
-#        # Create a new trajectory object
-#        new_traj = RobotTrajectory()
-       
-#        # Initialize the new trajectory to be the same as the input trajectory
-#        new_traj.joint_trajectory = traj.joint_trajectory
-       
-#        # Get the number of joints involved
-#        n_joints = len(traj.joint_trajectory.joint_names)
-       
-#        # Get the number of points on the trajectory
-#        n_points = len(traj.joint_trajectory.points)
-        
-#        # Store the trajectory points
-#        points = list(traj.joint_trajectory.points)
-       
-#        # Cycle through all points and joints and scale the time from start,
-#        # as well as joint speed and acceleration
-#        for i in range(n_points):
-#            point = JointTrajectoryPoint()
-           
-#            # The joint positions are not scaled so pull them out first
-#            point.positions = list(traj.joint_trajectory.points[i].positions)
-
-#            # Next, scale the time_from_start for this point
-#             # point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
-           
-#            # Get the joint velocities for this point
-#            point.velocities = list(traj.joint_trajectory.points[i].velocities)
-           
-#            # Get the joint accelerations for this point
-#            point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
-           
-#            # Scale the velocity and acceleration for each joint at this point
-#            for j in range(n_joints):
-#             #    if point.positions[j]
-#                if has_velocity_limits:
-#                 if point.velocities[j] > joint_max_velocity[j]:
-#                     vel_exception_point
-#                     rospy.loginfo("velocities Test OK") 
-#                 else:
-#                     raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")     
-#                if has_acceleration_limits:
-#                 point.accelerations[j] = point.accelerations[j] * scale * scale
-        
-#            # Store the scaled trajectory point
-#            points[i] = point
-
-#        rospy.loginfo("velocities Check OK") 
-
-#        # Assign the modified points to the new trajectory
-#        new_traj.joint_trajectory.points = points
-
-#        # Return the new trajecotry
-#        return new_traj
-
-#     else:
-#         print("traj type is not std")
-    
-def check_joint_positions(joint_trajectory):
-    is_limited = False
-    Limit_margin = True
-    percentage = 0
-    for i in range(len(joint_trajectory.joint_trajectory.points) - 1):
-        if  not is_limited:
-            joint_positions1 = joint_trajectory.joint_trajectory.points[i].positions
-            joint_positions2 = joint_trajectory.joint_trajectory.points[i + 1].positions 
-
-            for j in range(len(joint_positions1)):
-                position_diff = abs(joint_positions1[j] - joint_positions2[j])
-                if position_diff > 6:
-                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
-                    new_start_point = list(joint_trajectory.joint_trajectory.points[0].positions)
-                    
-                    #关节限位值
-                    joint_limt =[[],[],[],[-3.314,3.314],[],[-3.838,3.838]] 
-                    #检查关节限位余量
-                    # midpoint = (joint_limt[j][0] + joint_limt[j][1])/2
-                    # distance_to_midpoint = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- midpoint)
-                    Limit_point = joint_positions1[j]
-                    distance_to_Limit_point = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- Limit_point)
-                    Joint_range = abs(joint_limt[j][0] - joint_limt[j][1])
-                    margin = distance_to_Limit_point / Joint_range
-                    if margin > 0.4:
-                        Limit_margin = True
-                    else:
-                        Limit_margin = False
-                    rospy.loginfo("margin = {}".format(margin))
-                    # if joint_positions1[j]>0:
-                    #     new_start_point[j] -= np.pi*2
-                    # else:
-                    #     new_start_point[j] += np.pi*2
-                    # rospy.loginfo("new point{}:{}".format(i,new_start_point))
-                    percentage = i/len(joint_trajectory.joint_trajectory.points)
-                    is_limited = True
-                    break
-                if position_diff > 3:
-                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
-                    
-                    
-                    
-                    Limit_margin = True
-                    
-                    
-                    is_limited = True
-                    break
-        if  is_limited:
-            break
-    if not is_limited:
-        rospy.loginfo("joint positions check OK")         
-    return is_limited,Limit_margin
-
-def angle_adjustment(pose,axis,angle):
-    if len(pose) == 6:
-        q_initial = quaternion_from_euler(pose[3], pose[4], pose[5])
-    else:
-        q_initial = (pose[3], pose[4], pose[5],pose[6])
-    if axis == 'z':
-        q_rotation = quaternion_from_euler(0, 0, angle)
-    if axis == 'y':
-        q_rotation = quaternion_from_euler(0, angle, 0)
-    if axis == 'x':
-        q_rotation = quaternion_from_euler(angle, 0, 0)    
-    q_new = quaternion_multiply(q_initial, q_rotation)
-    pose[3:] = q_new
-    return pose
-
-
-

catkin_ws/src/lstrobot_planning/scripts/command.py

@@ -7,7 +7,7 @@ file_path = "moveitServer2"
 script_directory = os.path.dirname(os.path.abspath(__file__))
 
 def load_rviz():
-    command = "roslaunch lstrobot_moveit_config_0419 demo.launch"
+    command = "roslaunch lstrobot_moveit_config_0605 demo.launch"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def launch_publish_pointcloud():
     file_name = "dycl_0506.py"

catkin_ws/src/lstrobot_planning/scripts/dycl_0506.py

@@ -12,8 +12,6 @@ from sensor_msgs.msg import PointField
 folder_path = rospy.get_param("folder_path")
 file_path = os.path.join(folder_path, 'pointcloud.txt')
 file_path_points = os.path.join(folder_path, 'points.txt')
-file_path_down = os.path.join(folder_path, 'pointcloud_down.pcd')
-file_path_octomap = os.path.join(folder_path, 'pointcloud_octomap.pcd')
 #chen############################################################################################
 def cull_pointcloud(data,culling_radius):
     data_retained,data_culled = [],[]
@@ -38,11 +36,6 @@ def cull_pointcloud(data,culling_radius):
     data_culled = np.array(data_culled)
     return data_retained,data_culled    
 
-def visual_data(data):
-    # 可视化
-    pt_cloud_circular = o3d.geometry.PointCloud()
-    pt_cloud_circular.points = o3d.utility.Vector3dVector(data)
-    o3d.visualization.draw_geometries([pt_cloud_circular]) 
 ################################点云降采样################################
 
 # 从文件中读取焊缝起点和终点，并计算两点构成的向量
@@ -84,82 +77,27 @@ def load_point_cloud_from_binary_txt(file_path):
         point_cloud = np.copy(point_cloud)
     return point_cloud
 
-def open_file(file_path):
-    if '.txt' in file_path:
-        pcd = o3d.io.read_point_cloud(file_path, format='xyz')
-    else:
-        pcd = o3d.io.read_point_cloud(file_path)
-    return pcd
 
-def save_file(content, file_path, write_ascii=True):
-    if '.txt' in file_path:
-        np.savetxt(file_path, np.asarray(content.points), fmt='%f %f %f')
-    else:
-        o3d.io.write_point_cloud(file_path, content, write_ascii=write_ascii)
-
-def voxel_down_sample(pcd, voxel_size):
-    """
-    体素方法点云降采样
-    :param pcd: 点云数据
-    :param voxel_size: 体素降采样的网格长度
-    :return: 降采样点云数据
-    """
-    return pcd.voxel_down_sample(voxel_size=voxel_size)
-
-def add_rgb_to_pcd(pcd):
-
-    # 为点云中的每个点指定RGB颜色，这里我们使用红色作为示例
-    # RGB值范围为[0, 1]，红色可以表示为(1, 0, 0)
-    pcd = np.asarray(pcd.points).shape[0]
-    colors = np.repeat([[1, 0, 0]], pcd, axis=0)  # 创建一个全是红色的颜色数组
-    # 将颜色信息添加到点云对象中
-    pcd.colors = o3d.utility.Vector3dVector(colors)
-    return pcd
-
-# 降采样
-# pcd = open_file(file_path)
+
 
 pcd = o3d.geometry.PointCloud()
 pcd.points = o3d.utility.Vector3dVector(load_point_cloud_from_binary_txt(file_path))
 pcd = pcd.voxel_down_sample(voxel_size = 4)
-save_file(pcd,file_path_down)
 
 ################################去掉焊缝周围的点云#################################
-
 # 计算焊缝向量
-
 start_points,end_points,vectors = np.array(read_and_calculate_vectors(file_path_points))
-# 使用pcread直接读取点云数据
-ptCloud = o3d.io.read_point_cloud(file_path_down)
-data = np.asarray(ptCloud.points)
 
-culling_radius1 = 20
+data = np.asarray(pcd.points)
 culling_radius2 = float(rospy.get_param('culling_radius'))
-# culling_radius2 = 6
-
 data_retained2,data_culled2 =cull_pointcloud(data,culling_radius2)
-# rospy.loginfo('Point cloud culling is complete')
-#分批剔除
-# data_retained1,data_culled1 =cull_pointcloud(data,culling_radius1)
-# # visual_data(data_culled1)
-# data_retained2,data_culled2 =cull_pointcloud(data_culled1,culling_radius2)
-# visual_data(data_retained2)
-# visual_data(data_culled2)
-################################变换单位并保存################################
-
-# 将点云数据的小数点向前移动三位（即乘以1000）
-# data_scaled1 = np.array(data_retained1) / 1000
+
 data_scaled2 = np.array(data_retained2) / 1000
-# 创建一个新的pointCloud对象来存储缩放后的数据
+
 ptCloud_scaled1 = o3d.geometry.PointCloud()
-# ptCloud_scaled1.points = o3d.utility.Vector3dVector(data_scaled1)
+
 ptCloud_scaled2 = o3d.geometry.PointCloud()
 ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)
-# 将缩放后的点云数据写入新的 PCD 文件
-# o3d.io.write_point_cloud(file_path_octomap, ptCloud_scaled1, write_ascii=True)
-
-# 读取保存的点云数据
-# pcd = o3d.io.read_point_cloud(file_path_octomap)
 
 #####################################################################################################################################################################
 def build_pointcloud2_msg(points):
@@ -184,8 +122,8 @@ def build_pointcloud2_msg(points):
     msg.is_dense = False
     msg.data = np.asarray(points, np.float32).tobytes()
     return msg
+
 def talker():
-    pointcloud_topic = rospy.get_param('pointcloud_topic')
     pub1 = rospy.Publisher("/pointcloud/output", PointCloud2, queue_size=10)
     # pub2 = rospy.Publisher("/pointcloud/output2", PointCloud2, queue_size=10)
     rospy.init_node('publish_pointcloud', anonymous=True)

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -1,8 +1,7 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
 # 导入基本ros和moveit库
 from logging.handlers import RotatingFileHandler
 import os
+import moveit_msgs.msg
 import rospy, sys
 import moveit_commander      
 import moveit_msgs
@@ -23,23 +22,25 @@ import tf
 from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
 import math
 import traceback
-import check
 from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 
+
 pi = np.pi
 
 class MoveIt_Control:
     # 初始化程序
     def __init__(self, is_use_gripper=False):
         self.home_pose=[]
+        self.hf_num=0#焊缝计数 从0开始
+        self.hf_fail=[]
         # Init ros config
         moveit_commander.roscpp_initialize(sys.argv)
-        # 初始化ROS节点
-        rospy.init_node('moveit_control_server', anonymous=False)   
+        
         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()
         # 设置机械臂基座的参考系
@@ -49,26 +50,15 @@ class MoveIt_Control:
         # # 设置最大规划时间和是否允许重新规划
         self.arm.set_planning_time(10)
         self.arm.allow_replanning(True)
-        planner_id = rospy.get_param("planner_id")
-        # self.arm.set_planner_id(planner_id)
-        # self.arm.set_planner_id("RRTstar")
-        # self.arm.set_planner_id("SBL")
-        # self.arm.set_planner_id("BKPIECE")#速度快，比较稳定
-        # self.arm.set_planner_id("BiEST")#还不错
-        
-        # 设置允许的最大速度和加速度（范围：0~1）
-        robot_params = {
-            'velocity_limits': True,
-            'joint_max_velocity': [3.541, 3.541, 3.733, 6.838, 4.815, 23.655],
-            'acceleration_limits': False,
-            'joint_max_acceleration': [0, 0, 0, 0, 0, 0]
-        }
+
         self.arm.set_max_acceleration_scaling_factor(1)
         self.arm.set_max_velocity_scaling_factor(1)
 
+        self.arm.set_num_planning_attempts(10)
         # 机械臂初始姿态
+
         self.go_home()
-        home_pose=self.get_now_pose()
+        self.home_pose=self.get_now_pose()
         
     def get_now_pose(self):
         # 获取机械臂当前位姿
@@ -86,10 +76,6 @@ class MoveIt_Control:
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
     
-    def close(self):
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
     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]])
@@ -137,259 +123,57 @@ class MoveIt_Control:
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
         rrr=0.05#初始允许半径
-
-        
+ 
         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)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
-            self.arm.set_path_constraints(path_constraints)#设定约束
-            
+            path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
         else:
-            #起点位置设定为当前状态  按理来说是home点 
+            #刚开始规划 起点位置设定为当前状态  按理来说是home点 
             self.go_home()
             self.home_pose=self.get_now_pose()
-            #rospy.loginfo(self.home_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_path_constraints(path_constraints)#设定约束
         self.arm.set_pose_target(point, self.end_effector_link)#设定目标点为传入的点
         self.arm.set_start_state(state)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
         
-        
-        #当前起点规划次数
         #尝试规划次数
         b = 16 #尝试规划10次
         for i in range(b):
             traj = self.arm.plan()#调用plan进行规划
-            trajj = traj[1] #取出规划的信息
-            error,Limit_margin = check.check_joint_positions(trajj)#检查关节状态 就是检测规划的轨迹有没有“摇花手”
-            
-            if not error:#如果没有摇花手 接受规划
-                rospy.loginfo("本次移动 OK")
+            error=traj[3]
+            if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
+                rospy.loginfo("本次移动OK")
+                trajj = traj[1] #取出规划的信息
+                trajj_with_type = mark_the_traj(trajj,"during-p",welding_sequence)
+                trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
+                trajj_with_type.points[-1].type = "end"      
+                trajectory_with_type.append(trajj_with_type)
+                points.append(point)
+                trajectory.append(trajj)  
                 break
-            else: 
+            else:
                 rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
-                if i%2==0 and i!=0:#10次了都没成功 重新修改约束
+                self.arm.clear_path_constraints()
+                rrr=rrr+0.2#每次增加10厘米
+                rospy.loginfo("R值：{}次重新规划".format(rrr))
+                path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                self.arm.set_path_constraints(path_constraints)#设定约束
+                if(i==(b-1)):
+                    rospy.loginfo("所有移动规划尝试失败 规划器停止---请检查工件")
                     self.arm.clear_path_constraints()
-                    rrr=rrr+0.1#每次增加10厘米
-                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
-                    self.arm.set_path_constraints(path_constraints)#设定约束
-
-        #规划失败
-        if error:
-            rospy.loginfo("{}次移动规划失败，进程终止".format(b))
-            sys.exit() 
+                    sys.exit(0)
         #清除约束
         self.arm.clear_path_constraints()
         
-        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"      
-        trajectory_with_type.append(trajj_with_type)
-        points.append(point)
-        trajectory.append(trajj)    
         return points,trajectory,trajectory_with_type
     
-
-    # 测试程序用
-    def testRobot(self):
-        try:
-            rospy.loginfo("Test for robot...")
-           
-            file_path_result = os.path.join(folder_path, 'result.txt')
-            all_data = process_welding_data(file_path_result)
-
-            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]]
-                self.move_p(point11)
-                point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
-                self.move_p(point22)
-                rospy.loginfo("第%d条焊缝规划完毕",i+1)
-
-            # joint_position = [x * math.pi / 180 for x in [11.016, 3.716, -19.553, -5.375, -32.4173, 109.158]]
-            # self.move_j(joint_position)
-            # self.show_current_pose()
-            
-            print("Test OK")
-            rospy.sleep(1)
-            
-        except Exception as e:
-            print("Test fail! Exception:", str(e))
-            traceback.print_exc()
-        # except:
-        #     print("Test fail! ")
-
-    def show_current_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)
-        # 打印位姿信息
-
-        rospy.loginfo("Current Pose: {}".format(pose))
-
-    def set_scene(self):
-        rospy.loginfo("set scene")
-        ## set table
-        self.scene = PlanningSceneInterface()
-        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
-        self.colors = dict()
-        # rospy.sleep(1)
-        table_id = 'table'
-        self.scene.remove_world_object(table_id)
-        rospy.sleep(1)
-        table_size = [2, 2, 0.01]
-        table_pose = PoseStamped()
-        table_pose.header.frame_id = self.reference_frame
-        table_pose.pose.position.x = 0.0
-        table_pose.pose.position.y = 0.0
-        table_pose.pose.position.z = -table_size[2]/2
-        table_pose.pose.orientation.w = 1.0
-
-        box2_id = 'box2'
-        self.scene.remove_world_object(box2_id)
-        rospy.sleep(1)
-        box2_size = [0.01, 0.6, 0.4]
-        box2_pose = PoseStamped()
-        box2_pose.header.frame_id = self.reference_frame
-        box2_pose.pose.position.x = 1.15194-box2_size[0]/2-0.005
-        box2_pose.pose.position.y = -0.707656
-        box2_pose.pose.position.z = 0.4869005
-        box2_pose.pose.orientation.w = 1.0
-
-        box3_id = 'box3'
-        self.scene.remove_world_object(box3_id)
-        rospy.sleep(1)
-        box3_size = [0.2, 0.01, 0.4]
-        box3_pose = PoseStamped()
-        box3_pose.header.frame_id = self.reference_frame
-        box3_pose.pose.position.x = 1.15194+box3_size[0]/2
-        box3_pose.pose.position.y = -0.707656-box3_size[1]/2-0.005
-        box3_pose.pose.position.z = 0.4869005
-        box3_pose.pose.orientation.w = 1.0
-
-        # self.scene.add_box(table_id, table_pose, table_size)
-        # self.scene.add_box(box2_id, box2_pose, box2_size)
-        # self.scene.add_box(box3_id, box3_pose, box3_size)
-
-        self.setColor(table_id, 0.5, 0.5, 0.5, 1.0)
-        self.sendColors()
-
-        rospy.loginfo("set scene end")
-
-    def plan_cartesian_path(self,waypoints):
-        fraction = 0.0  # 路径规划覆盖率
-        maxtries = 100  # 最大尝试规划次数
-        attempts = 0  # 已经尝试规划次数
-
-        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
-        while fraction < 1.0 and attempts < maxtries:
-            (plan, fraction) = self.arm.compute_cartesian_path(
-                waypoints,  # waypoint poses，路点列表
-                0.001,  # eef_step，终端步进值
-                0.0,  # jump_threshold，跳跃阈值
-                True)  # avoid_collisions，避障规划
-            attempts += 1
-        if fraction == 1.0:
-            rospy.loginfo("Path computed successfully.")
-            traj = plan
-        else:
-            rospy.loginfo(
-                "Path planning failed with only " + str(fraction) +
-                " success after " + str(maxtries) + " attempts.")   
-        return fraction,plan
-    
-    # 关节规划，输入6个关节角度（单位：弧度）
-    def move_j(self, joint_configuration=None,a=1,v=1):
-        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
-        if joint_configuration==None:
-            joint_configuration = [0, -1.5707, 0, -1.5707, 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)
-
-    # 空间规划，输入xyzRPY
-    def move_p(self, tool_configuration=None,a=1,v=1):
-        if tool_configuration==None:
-            tool_configuration = [0.3,0,0.3,0,-np.pi/2,0]
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = 'base_link'
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = tool_configuration[0]
-        target_pose.pose.position.y = tool_configuration[1]
-        target_pose.pose.position.z = tool_configuration[2]
-        if len(tool_configuration) == 6:
-            q = quaternion_from_euler(tool_configuration[3],tool_configuration[4],tool_configuration[5])
-            target_pose.pose.orientation.x = q[0]
-            target_pose.pose.orientation.y = q[1]
-            target_pose.pose.orientation.z = q[2]
-            target_pose.pose.orientation.w = q[3]
-        else:
-            target_pose.pose.orientation.x = tool_configuration[3]
-            target_pose.pose.orientation.y = tool_configuration[4]
-            target_pose.pose.orientation.z = tool_configuration[5]
-            target_pose.pose.orientation.w = tool_configuration[6]
-
-        self.arm.set_start_state_to_current_state()
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-        rospy.loginfo("move_p:" + str(tool_configuration))
-        traj = self.arm.plan()
-        path = traj[1]
-        # file_path = "/home/chen/catkin_ws/src/lstrobot_planning/date/p_traj.txt"
-        # if os.path.exists(file_path):
-        #     os.remove(file_path)
-        # with open(file_path, "w") as file:
-        #     file.write(str(path))
-        self.arm.execute(path)
-        # rospy.sleep(1)
-
-    def move_p2(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)
-        # self.arm.set_start_state_to_current_state()
-        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)
-        rospy.loginfo("move_p:" + str(point))
-        traj = self.arm.plan()
-        trajj = traj[1]
-        error_code =traj[3]
-        rospy.loginfo("Move to start point planned successfully")
-
-        trajj_with_type = mark_the_traj(trajj,"during-p",welding_sequence)
-                   
-        points.append(point)
-        trajectory.append(trajj)
-        trajectory_with_type.append(trajj_with_type)
-        return points,trajectory,trajectory_with_type
     
-    def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
+    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)
 
@@ -398,7 +182,7 @@ class MoveIt_Control:
             #起点位置设置为规划组最后一个点
             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_start_state(state)
         
         self.arm.set_pose_target(point, self.end_effector_link)#设定目标点为传入的点
         
@@ -406,42 +190,26 @@ class MoveIt_Control:
         path_constraints = self.create_path_constraints(points[-1],point)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
         self.arm.set_path_constraints(path_constraints)#设定约束
         
-        #当前起点规划次数
-        #尝试规划次数
-        b = 20 #最多尝试规划10次
-        for i in range(b):
-            traj = self.arm.plan()#调用plan进行规划
+        traj = self.arm.plan()#调用plan进行规划
+        error=traj[3]
+        if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
+            rospy.loginfo("本次焊缝规划 OK")
             trajj = traj[1] #取出规划的信息
-            error,Limit_margin = check.check_joint_positions(trajj)#检查关节状态 就是检测规划的轨迹有没有“摇花手”
-            
-            if not error:#如果没有摇花手 接受规划
-                rospy.loginfo("本次焊缝规划 OK")
-                break
-            else: 
-                rospy.loginfo("焊缝规划失败-开始第{}次重新规划".format(i+1))
-                # if i%10==0 and i!=0:#10次了都没成功 将这个点用p2规划
-                #     prePoint = points.pop()#移除列表中最后一个元素  返回被移除的对象
-                #     trajectory.pop()
-                #     trajectory_with_type.pop()
-                #     #清除约束
-                #     self.arm.clear_path_constraints()
-                #     points,trajectory,trajectory_with_type = self.move_p2(prePoint,points,trajectory,trajectory_with_type)
-                #     rospy.loginfo("第{}次尝试使用p2".format((i/10)%1))
-
-        #规划失败
-        if error:
-            rospy.loginfo("{}次焊缝规划失败，进程终止".format(b))
-            sys.exit() 
+            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()#将本条焊缝的起点从规划中删除
+            trajectory.pop()
+            trajectory_with_type.pop()
+            self.hf_fail.append(welding_sequence[self.hf_num])
         #清除约束
         self.arm.clear_path_constraints()
-
-        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"      
-        #如果调用了P2函数 那么这段代码不会导致重复添加？
-        points.append(point)
-        trajectory.append(trajj)
-        trajectory_with_type.append(trajj_with_type)
+        self.hf_num=self.hf_num+1#焊缝计数
         return points,trajectory,trajectory_with_type
 
     def create_path_constraints(self,start_point,end_point):
@@ -482,150 +250,17 @@ class MoveIt_Control:
         position_constraint.constraint_region.primitive_poses.append(pose)
         position_constraint.weight = 1.0
 
-        # 姿态约束
-        # orientation_constraint = OrientationConstraint()
-        # orientation_constraint.header.frame_id = "base_link"
-        # orientation_constraint.link_name = self.end_effector_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
-
-    
-    def move_l(self, tool_configuration,waypoints_number=1,a=0.5,v=0.5):
-        if tool_configuration==None:
-            tool_configuration = [0.3,0,0.3,0,-np.pi/2,0]
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-
-        # 设置路点
-        waypoints = []
-        for i in range(waypoints_number):
-            target_pose = PoseStamped()
-            target_pose.header.frame_id = self.reference_frame
-            target_pose.header.stamp = rospy.Time.now()
-            target_pose.pose.position.x = tool_configuration[6*i+0]
-            target_pose.pose.position.y = tool_configuration[6*i+1]
-            target_pose.pose.position.z = tool_configuration[6*i+2]
-            q = quaternion_from_euler(tool_configuration[6*i+3],tool_configuration[6*i+4],tool_configuration[6*i+5])
-            target_pose.pose.orientation.x = q[0]
-            target_pose.pose.orientation.y = q[1]
-            target_pose.pose.orientation.z = q[2]
-            target_pose.pose.orientation.w = q[3]
-            waypoints.append(target_pose.pose)
-        rospy.loginfo("move_l:" + str(tool_configuration))
-        self.arm.set_start_state_to_current_state()
-        fraction = 0.0  # 路径规划覆盖率
-        maxtries = 100  # 最大尝试规划次数
-        attempts = 0  # 已经尝试规划次数
-
-        # 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
-        while fraction < 1.0 and attempts < maxtries:
-            (plan, fraction) = self.arm.compute_cartesian_path(
-                waypoints,  # waypoint poses，路点列表
-                0.001,  # eef_step，终端步进值
-                0.00,  # jump_threshold，跳跃阈值
-                True)  # avoid_collisions，避障规划
-            attempts += 1
-        if fraction == 1.0:
-            rospy.loginfo("Path computed successfully. Moving the arm.")
-            self.arm.execute(plan)
-            rospy.loginfo("Path execution complete.")
-        else:
-            rospy.loginfo(
-                "Path planning failed with only " + str(fraction) +
-                " success after " + str(maxtries) + " attempts.")
-        rospy.sleep(1)
-
-    def move_l22(self,point,points,trajectory,trajectory_with_type,a=0.5,v=0.5):
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-
-        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)
-        
-        start_rotation_angle,end_rotation_angle = 0,0
-        new_start_pose,new_end_pose = 0,0
-        num_planed = 0
-        while not start_rotation_angle == 2*pi:
-            waypoints = []
-            target_pose = point_to_PoseStamped(point,self.reference_frame)
-            waypoints.append(target_pose.pose)
-            rospy.loginfo("move_l2:" )
-        
-            num_remake = 2
-            for _ in range(num_remake):
-                num_planed +=1
-                rospy.loginfo("第{}次规划".format(num_planed))
-                rospy.loginfo("new_start_pose:{} new_end_pose:{}".format(new_start_pose,new_end_pose))
-                fraction,traj = self.plan_cartesian_path(waypoints)
-                if fraction == 1:
-                    error,percentage = check.check_joint_positions(traj)
-                    if not error:
-                        break
-            if fraction != 1:
-                break
-            error,percentage = check.check_joint_positions(traj)
-            if error:
-                #三次重规划结束，仍然不符合，插点或转点
-                angle_list = ([0,pi/4,-pi/4,pi/2,-pi/2,pi*3/4,-pi*3/4,pi,2*pi])
-                if start_rotation_angle < pi: 
-                    if end_rotation_angle < pi:
-                        index = angle_list.index(end_rotation_angle)
-                        end_rotation_angle = angle_list[index+1]
-                        new_end_pose = check.angle_adjustment(point,'z', start_rotation_angle)  # 将目标位姿旋转一定角度
-                        point = new_end_pose # 将新的点替换原来位姿
-                    else:
-                        index = angle_list.index(start_rotation_angle)
-                        start_rotation_angle = angle_list[index+1]
-                        new_start_pose = check.angle_adjustment(points[-1],'z', start_rotation_angle)  # 将目标位姿旋转一定角度
-                        #go home,plan p
-                        state = self.arm.get_current_state()
-                        state.joint_state.position = trajectory[-1].joint_trajectory.points[0].positions
-                        self.arm.set_start_state(state)
-                        del points[-1]
-                        del trajectory[-1]
-                        points,trajectory,trajectory_with_type = self.move_p2(new_start_pose,points,trajectory,trajectory_with_type)
-                        state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
-                        self.arm.set_start_state(state)
-                        
-                        end_rotation_angle = 0
-  
-            else:
-                break
-        if fraction != 1:
-            rospy.loginfo("Plan Fail,Error code 1")
-            sys.exit() 
-        elif error:
-            rospy.loginfo("Plan Fail,Error code 2")
-            sys.exit()
-        else:
-            traj_with_type = mark_the_traj(traj,"during-l",welding_sequence)
-            traj_with_type.points[-len(traj.joint_trajectory.points)].type = "start"  
-            traj_with_type.points[-1].type = "end"         
-            
-            points.append(point)
-            trajectory.append(traj)
-            trajectory_with_type.append(traj_with_type) 
-            return points,trajectory,trajectory_with_type
-        
+       
     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)
         # “up”为自定义姿态，你可以使用“home”或者其他姿态
         self.arm.set_named_target('home')
+        rospy.loginfo("get_home end")
         self.arm.go()
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
@@ -644,30 +279,6 @@ class MoveIt_Control:
         trajectory.append(trajj)
         trajectory_with_type.append(traj_with_type)
         return trajectory,trajectory_with_type
-    
-    def setColor(self, name, r, g, b, a=0.9):
-        # 初始化moveit颜色对象
-        color = ObjectColor()
-        # 设置颜色值
-        color.id = name
-        color.color.r = r
-        color.color.g = g
-        color.color.b = b
-        color.color.a = a
-        # 更新颜色字典
-        self.colors[name] = color
-
-    # 将颜色设置发送并应用到moveit场景当中
-    def sendColors(self):
-        # 初始化规划场景对象
-        p = PlanningScene()
-        # 需要设置规划场景是否有差异
-        p.is_diff = True
-        # 从颜色字典中取出颜色设置
-        for color in self.colors.values():
-            p.object_colors.append(color)
-        # 发布场景物体颜色设置
-        self.scene_pub.publish(p)
 
     def path_planning(self,folder_path,gohome=True):
         file_path_result = os.path.join(folder_path, 'result.txt')
@@ -693,72 +304,8 @@ class MoveIt_Control:
         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
-    def path_planning_test(self,folder_path,i,reverse=False,gohome=True):
-        file_path_result = os.path.join(folder_path, 'result.txt')
-        all_data = process_welding_data(file_path_result)
 
-        points,trajectory,trajectory_with_type = [],[],[]
-        i -= 1
-        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]
-        if reverse:
-            start_point = all_data[i][1]
-            end_point = all_data[i][0]
-            q1 = all_data[i][3]
-            q2 = all_data[i][2]
-
-        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 = self.move_p2(point11,points,trajectory,trajectory_with_type)
-        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)
-        rospy.loginfo("第%d条焊缝规划完毕",i+1)
-        if gohome:
-            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
-def point_to_PoseStamped(point,reference_frame):
-    target_pose = PoseStamped()
-    target_pose.header.frame_id = reference_frame
-    target_pose.header.stamp = rospy.Time.now()
-    target_pose.pose.position.x = point[0]
-    target_pose.pose.position.y = point[1]
-    target_pose.pose.position.z = point[2]
-    if len(point) == 6:
-        q = quaternion_from_euler(point[3],point[4],point[5])
-        target_pose.pose.orientation.x = q[0]
-        target_pose.pose.orientation.y = q[1]
-        target_pose.pose.orientation.z = q[2]
-        target_pose.pose.orientation.w = q[3]
-    else:
-        target_pose.pose.orientation.x = point[3]
-        target_pose.pose.orientation.y = point[4]
-        target_pose.pose.orientation.z = point[5]
-        target_pose.pose.orientation.w = point[6]
-
-    return target_pose
-def calculate_waypoints(start_pose,end_pose,spacing):
-    start_position = np.array([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z])
-    start_orientation = euler_from_quaternion([start_pose.pose.orientation.x,start_pose.pose.orientation.y,start_pose.pose.orientation.z,start_pose.pose.orientation.w])
-    end_position = np.array([end_pose.pose.position.x, end_pose.pose.position.y, end_pose.pose.position.z])
-    end_orientation = euler_from_quaternion([end_pose.pose.orientation.x,end_pose.pose.orientation.y,end_pose.pose.orientation.z,end_pose.pose.orientation.w])
-
-
-    distance = np.linalg.norm(np.array(end_position) - np.array(start_position))
-    num_points = int(distance/spacing)
-    trajectory = []
-    for i in range(num_points + 1):
-        alpha = float(i) / num_points
-        position = [start + alpha * (end - start) for start, end in zip(start_position, end_position)]
-        orientation = [start + alpha * (end - start) for start, end in zip(start_orientation, end_orientation)]
-        trajectory.append(position + orientation)
-
-    return trajectory,num_points + 1
- 
+###########################################################class end#############################################################
 def process_welding_data(filename):
     all_data = []  # 用来存储每一行处理后的数据
     with open(filename, 'r') as file:
@@ -791,6 +338,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
         ) for point in traj.joint_trajectory.points
     ]
     return traj_with_type
+
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -818,6 +366,7 @@ def merge_trajectories(trajectories):
             last_time_from_start = traj.joint_trajectory.points[-1].time_from_start + last_time_from_start
     
     return merged_trajectory
+
 def merge_trajectories_with_type(trajectory_with_type):
     if not trajectory_with_type:
         return None
@@ -845,6 +394,7 @@ def merge_trajectories_with_type(trajectory_with_type):
             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)
@@ -863,22 +413,47 @@ def pub_trajectories(trajectory_with_type_merge):
         # count += 1
         rate.sleep()
 
+
+class py_msgs():
+    fial=[]
+    shun_xv=[]
+    class point():
+        xyz_list=[]
+        type=[]
+
+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])
+    print(py_msgs.shun_xv)
+    print(py_msgs.fial)
+
+
+
 if __name__ =="__main__":
 
     folder_path = rospy.get_param("folder_path")
+    # 初始化ROS节点
+    rospy.init_node('moveit_control_server', anonymous=False)   
     moveit_server = MoveIt_Control(is_use_gripper=False)
-    moveit_server.arm.set_num_planning_attempts(10)
     
-    #获取规划信息，并规划
-    # folder_path = ("/home/chen/catkin_ws/data/0412")
     welding_sequence = rospy.get_param('welding_sequence')
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
     # trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning_test(folder_path,1,True)
     #执行动作
+
+    ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
+
     moveit_server.arm.execute(trajectory_merge)
     
+
     # for i in range(1 * 2 - 0): #执行到第i条焊缝
     #     moveit_server.arm.execute(trajectory[i])
     
     #发布规划完毕的轨迹信息
-    pub_trajectories(trajectory_with_type_merge)
+    #pub_trajectories(trajectory_with_type_merge)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -14,8 +14,6 @@ from rospy.exceptions import ROSException
 from std_srvs.srv import Empty
 from rospy.service import ServiceException
 
-waited_once = False
-
 def wait_for_topic(topic_name, message_type):
     try:
         rospy.loginfo("等待场景地图加载完毕...")
@@ -34,32 +32,29 @@ def clear_octomap():
     except ServiceException as e:
         rospy.loginfo("Failed to call /clear_octomap service or it is not available. Skipping operation.")
 
+waited_once = False
 if __name__ == "__main__":
+    command.load_rviz()
     rospy.init_node("set_update_paramter_p")
 
     # 初始化各种类型参数
-    rospy.set_param("sequence_ready",0)
     rospy.set_param("sign_control",0)
     rospy.set_param("sign_pointcloud",0)
     rospy.set_param("sign_traj_accepted",0)
+
     rospy.set_param("yaw",np.pi/6)  #内收角（偏航角）
     rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
     rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
     rospy.set_param("pitch_of_Verticalweld",np.pi/5)
-    rospy.set_param("planner_id","RRTConnect")
-    rospy.set_param("pointcloud_topic","/pointcloud/output")
     rospy.set_param("culling_radius",7) #焊缝剔除半径
-    # rospy.set_param("/move_group/octomap_resolution",0.006)
-    rospy.set_param("folder_path","/home/chen/catkin_ws/src/publish_pointcloud/publish_pointcloud/data")
+    rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
 
-    # 显示参数当前值
-    sign_pointcloud = rospy.get_param("sign_pointcloud")
-
-    rospy.loginfo("当前参数值:")
-    rospy.loginfo("sign_pointcloud = %s",sign_pointcloud)
+    rospy.loginfo("当前参数值:")# 显示参数当前值
+    rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
     rospy.loginfo("yaw = %s",rospy.get_param("yaw")/np.pi*180)
     rospy.loginfo("pitch_of_Horizontalweld = %s",rospy.get_param("pitch_of_Horizontalweld")/np.pi*180)
     rospy.loginfo("pitch_of_Verticalweld = %s",rospy.get_param("pitch_of_Verticalweld")/np.pi*180)
+    
     while True:
         sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
@@ -67,32 +62,24 @@ if __name__ == "__main__":
                 print("等待点云数据准备完成...")
                 waited_once = True
         elif sign_control == "1":
-            # 初始化参数
+            # 重置参数
             rospy.set_param("sign_control",0)
             rospy.set_param("sign_traj_accepted",0)
-            
-            # 获取新的文件夹值
-            new_folder_path = rospy.get_param("folder_path")
-            rospy.loginfo("folder_path:%s",new_folder_path)
-            
             # 清除场景
             clear_octomap()
-
-            rospy.loginfo("launch pointcloud node now")
+            #点云发布
             process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            # 启动子进程
             process.start()
-            # command.launch_publish_pointcloud()
-
+            #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()
 
             # 等待 /move_group/monitored_planning_scene 话题发布消息
-            planning_scene = wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
             rospy.set_param("sign_pointcloud",1)
-            rospy.loginfo("终止点云发布，关闭发布窗口")
+            #rospy.loginfo("终止点云发布，关闭发布窗口")
             
-            rospy.loginfo("launch moveitserver node now")
+            rospy.loginfo("运行moveitserver2.py")
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
             
@@ -102,12 +89,11 @@ if __name__ == "__main__":
         elif sign_control == "2":
             # 重置参数
             rospy.set_param("sign_control",0)
+            rospy.set_param("sign_traj_accepted",0)
 
-            rospy.set_param("planner_id","BiEST")
-            rospy.loginfo("launch moveitserver node now")
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
-            rospy.set_param("planner_id","RRTConnect")
+
             #运行结束，重置参数
             waited_once = False
         

catkin_ws/更新记录.txt

@@ -0,0 +1,19 @@
+2024-7-31
+优化了轨迹规划：
+	做了关节限位 规划时不会出现大角度差姿态来回切换
+	做了轨迹间路径约束 移动时末端执行器不会走冗余路径
+	
+2024-8-5
+	修改rvizload
+	修改点云加载 不再保存又立即读出
+	修改轨迹规划 去除check 转而判断规划成功与否
+	增加ROS转py成员变量接口
+	ROS消息转PY类
+	增加不重新加载点云和焊接顺序姿态等  只重新规划机器人路径和运动学逆解的接口
+	
+	
+	
+	
+	做初始化优化   go home (搞不了 是系统封装的函数   而且好像是因为第一次要加载点云到碰撞检测)
+	因为是多文件启动 要防止规划器被2次启动
+