catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Planned Path1
         - /RobotModel1/Links1
         - /Trajectory1/Links1
@@ -157,7 +158,7 @@ Visualization Manager:
         Scene Alpha: 1
         Scene Color: 50; 230; 50
         Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
+        Show Scene Geometry: false
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
@@ -350,16 +351,7 @@ Visualization Manager:
       Marker Topic: /rviz_visual_tools
       Name: MarkerArray
       Namespaces:
-        Path: true
-        Text: true
-        point0  0.978994  -0.262691  0.279523: true
-        point0  0.980265  0.126875  0.278115: true
-        point0  0.983265  0.126875  0.278115: true
-        point0  0.983265  0.126875  0.473115: true
-        point1  0.978378  0.122593  0.278064: true
-        point1  0.983265  -0.262875  0.473115: true
-        point1  0.983265  0.126875  0.468115: true
-        point1  1.125382  0.127069  0.280059: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/TF
@@ -395,7 +387,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.102827310562134
+      Distance: 0.9001314043998718
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -403,17 +395,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.3195676803588867
-        Y: 0.5246121883392334
-        Z: 0.8483012914657593
+        X: 0.877734899520874
+        Y: -0.03542045131325722
+        Z: 0.13408130407333374
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4452030062675476
+      Pitch: 0.42520225048065186
       Target Frame: base_link
-      Yaw: 4.942476749420166
+      Yaw: 5.237476825714111
     Saved: ~
 Window Geometry:
   Displays:

catkin_ws/src/lstrobot_planning/scripts/check.py

@@ -0,0 +1,145 @@
+import rospy, sys
+import numpy as np
+from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
+
+#####检查关节限位#####
+def check_joint_positions(joint_trajectory):
+    is_limited = False      #初始化为满足限制条件
+    Limit_margin = True     #限制边界，初始化为True，用来检查是否超出限制条件
+    
+    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)):    #遍历但前轨迹点的每个关节点，这里是joint_positions1
+                position_diff = abs(joint_positions1[j] - joint_positions2[j])  #计算相邻关节轨迹点的关节点绝对差值，归一化到[-pi,pi]
+                
+                if position_diff > 6:   #限位阈值，6rad=343.8度
+                    rospy.loginfo("发生大角度翻转: point{}-joint{}:{}".format(i,j+1,joint_positions1))
+                    
+                    #关节限位值
+                    joint_limt =[[],[],[],[-1.6,1.6],[],[-3.838,3.838]] 
+                    
+                    #设定限位点为当前关节点
+                    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.3:        
+                        Limit_margin = True  
+                    else:
+                        Limit_margin = False
+                    rospy.loginfo("margin = {}".format(margin))
+                    is_limited = True           
+                    break
+                
+                if position_diff > 3:    #限位阈值，3rad=171.9度
+                    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("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])     #len>6，默认为四元数
+    if axis == 'z':
+        q_rotation = quaternion_from_euler(0, 0, angle)     #绕z轴旋转，转换得到关于z轴的旋转四元数
+    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)      #q_new为调整后的姿态四元数，值初始姿态四元数乘以旋转四元数
+    pose[3:] = q_new    #将q_new放入pose列表中的3号索引以后的位置
+    return pose
+
+
+
+# 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")
+    
+
+
+

catkin_ws/src/lstrobot_planning/scripts/decorator_time.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python3
+import rospy
+import time
+
+#定义一个装饰器，用来计算函数运行时间
+#使用方法：在需要计算运行时间的函数前面加上@decorator_time
+
+def decorator_time(func):
+    def wrapper(*args, **kwargs):
+        start_time = time.time()
+        result = func(*args, **kwargs)
+        end_time = time.time()
+        elapsed_time = end_time - start_time
+        rospy.loginfo(f"Time taken by {func.__name__}: {elapsed_time:.4f} seconds")
+        return result
+    return wrapper

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -25,6 +25,8 @@ import traceback
 from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
+from decorator_time import decorator_time
+import check
 
 pi = np.pi
 
@@ -36,6 +38,7 @@ class MoveIt_Control:
         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)
@@ -88,7 +91,8 @@ class MoveIt_Control:
         # 打印位姿信息       
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
-        
+    
+    #TODO move_p_path_constraints    
     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]])
@@ -130,6 +134,7 @@ class MoveIt_Control:
         # constraints.orientation_constraints.append(orientation_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)
@@ -157,39 +162,43 @@ class MoveIt_Control:
             traj = self.arm.plan()#调用plan进行规划
             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:
-                rospy.loginfo(error)
-                rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
-                self.arm.clear_path_constraints()
-                rrr=rrr+0.2#每次增加20厘米半径
-                rospy.loginfo("R值：{}".format(rrr))
-                if trajectory:
-                    path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                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)
+                    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:
-                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
-                self.arm.set_path_constraints(path_constraints)#设定约束
-                if(i>=(b-5)):
-                    rospy.loginfo("所有移动规划尝试失败 取消路径约束")
+                    rospy.loginfo("check failed! 移动轨迹无效")
+                    rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                     self.arm.clear_path_constraints()
-                if(i==(b-1)):
-                    rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
-                    er=1
-                    break
+                    rrr=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  
         #清除约束
         self.arm.clear_path_constraints()
         
         return points,trajectory,trajectory_with_type,er
     
-    
+    #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)
@@ -197,21 +206,22 @@ class MoveIt_Control:
 
         #设定约束
         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)#设定约束
-        
+
         traj = self.arm.plan()#调用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"  
@@ -224,17 +234,19 @@ class MoveIt_Control:
             points.pop()#将本条焊缝的起点从规划中删除
             trajectory.pop()
             trajectory_with_type.pop()
-            self.hf_fail.append(welding_sequence[self.hf_num])
+            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
 
+    
+    #TODO move_pl_path_constraints
     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.001
+        radius = 0.01
 
         constraints = Constraints()
         
@@ -271,7 +283,9 @@ class MoveIt_Control:
         constraints.position_constraints.append(position_constraint)
 
         return constraints
-       
+     
+    #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)
@@ -282,6 +296,7 @@ class MoveIt_Control:
         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)
@@ -296,6 +311,8 @@ class MoveIt_Control:
         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)
@@ -309,16 +326,17 @@ class MoveIt_Control:
             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,v=speed_v)
+            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)
+            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)
@@ -345,6 +363,7 @@ def process_welding_data(filename):
 
     return all_data
 
+#TODO mark_the_traj
 def mark_the_traj(traj,TYPE,SEQUENCE):
     traj_with_type = JointTrajectory_ex()
     traj_with_type.header = traj.joint_trajectory.header
@@ -361,6 +380,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
+#TODO merge_trajectories
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -389,6 +409,7 @@ def merge_trajectories(trajectories):
     
     return merged_trajectory
 
+#TODO merge_trajectories_with_type
 def merge_trajectories_with_type(trajectory_with_type):
     if not trajectory_with_type:
         return None
@@ -472,10 +493,10 @@ def get_valid_input(factor, default):
             return default
         try:
             value=float(user_input)
-            if 0<=value<=1:
+            if 0<value<=1:
                 return value
             else:
-                rospy.logerr("缩放因子必须在[0, 1]范围内，请重新输入!")
+                rospy.logerr("缩放因子必须在(0, 1]范围内，请重新输入!")
         except ValueError:
             rospy.logerr("输入值无效，请根据提示重新输入！")
 
@@ -495,8 +516,10 @@ def get_user_input():
         return vv
     except Exception as e:
         rospy.logerr(f"发生错误：{e}")
-
         
+
+            
+
 if __name__ =="__main__":
     # from redis_publisher import Publisher
     # publisher = Publisher()
@@ -506,12 +529,13 @@ if __name__ =="__main__":
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-
-    speed_v=get_user_input()
-    rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
+    speed_v=1
+    # speed_v=get_user_input()
+    # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
-
+   
+    
     moveit_server.arm.execute(trajectory_merge)
     
     rospy.set_param("sign_control",0)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -46,13 +46,19 @@ if __name__ == "__main__":
 
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
-    rospy.set_param("sign_traj_accepted",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("culling_radius",0) #焊缝剔除半径
+
+    # rospy.set_param("yaw",0)  #内收角（偏航角）
+    # rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
+    # rospy.set_param("pitch_of_Horizontalweld",0)  #和底面夹角（俯仰角）
+    # rospy.set_param("pitch_of_Verticalweld",0)
+
+    #rospy.set_param("culling_radius",0) #焊缝剔除半径
     rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
@@ -67,9 +73,11 @@ if __name__ == "__main__":
         sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
             if not waited_once:
-                print("等待点云数据准备完成...")
+                #print("等待点云数据准备完成...")
                 sys.stdin.flush()#清空键盘缓冲区
-                aaa=input("请选择 1-2: ")
+                aaa=input("请选择 1-2 默认为2: ")
+                if aaa=="":
+                    aaa='2'
                 #aaa='2'
 
                 rospy.set_param("cishu",rospy.get_param("cishu")+1)
@@ -82,7 +90,7 @@ if __name__ == "__main__":
         elif sign_control == "1":
             # 重置参数
             #rospy.set_param("sign_control",0)    #在轨迹规划里重置 防止重复调用
-            rospy.set_param("sign_traj_accepted",0)
+            #rospy.set_param("sign_traj_accepted",0)
             # 清除场景
             clear_octomap()
             #点云计算并发布
@@ -93,9 +101,9 @@ if __name__ == "__main__":
             hanqiangpose.run()
             command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
-            rospy.loginfo("等待场景地图加载完毕...")
+            #rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            rospy.loginfo(f"场景地图已加载完毕")
+            #rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")
             
@@ -113,21 +121,23 @@ if __name__ == "__main__":
             rospy.set_param("time",0)
             # 重置参数
             #rospy.set_param("sign_control",0)
-            rospy.set_param("sign_traj_accepted",0)
+            #rospy.set_param("sign_traj_accepted",0)
             hjsx.run()
             hanqiangpose.run()
+            command.load_visual()
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
             while rospy.get_param("sign_control"):
                 pass
 
-            tim=time.time()-tim
-            tim_list.append(tim)
-            print("-------------------------------------")
-            print(f'第{rospy.get_param("cishu")}次规划：')
-            print(tim_list)
-            print("")
+            # tim=time.time()-tim
+            # tim_list.append(tim)
+            # print("-------------------------------------")
+            # print(f'第{rospy.get_param("cishu")}次规划：')
+            # print(tim_list)
+            # print("")
+            
             #运行结束，重置参数
             waited_once = False
 

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -387,8 +387,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-2.1"
-      upper="2.1"
+      lower="-2.8"
+      upper="2.8"
       effort="32.64"
       velocity="23.655" />
   </joint>

catkin_ws/src/visual/src/visual_tools.cpp

@@ -147,20 +147,60 @@ int main(int argc, char** argv)
 		  //   std::cout << l[i]<< " ";
 	    // }
       // std::cout<<std::endl;
+
     }
 
   }
   file.close(); // 关闭文件
   
+  // double xxx,yyy,zzz,xxx2,yyy2,zzz2,xxx3,yyy3,zzz3;
+  // double qx,qy,qz,qw;
+  // ros::param::get("xxx", xxx);
+  // ros::param::get("yyy", yyy);
+  // ros::param::get("zzz", zzz);
+
+  // ros::param::get("qx", qx);
+  // ros::param::get("qy", qy);
+  // ros::param::get("qz", qz);
+  // ros::param::get("qw", qw);
+
+  // target_pose2.position.x = xxx/1000;
+  // target_pose2.position.y = yyy/1000;
+  // target_pose2.position.z = zzz/1000;
+  // target_pose2.orientation.x= qx;
+  // target_pose2.orientation.y= qy;
+  // target_pose2.orientation.z= qz;
+  // target_pose2.orientation.w = qw;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
+  // ros::param::get("xxx2", xxx2);
+  // ros::param::get("yyy2", yyy2);
+  // ros::param::get("zzz2", zzz2);
+  // target_pose2.position.x = xxx2/1000;
+  // target_pose2.position.y = yyy2/1000;
+  // target_pose2.position.z = zzz2/1000;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
+  // ros::param::get("xxx3", xxx3);
+  // ros::param::get("yyy3", yyy3);
+  // ros::param::get("zzz3", zzz3);
+  // target_pose2.position.x = xxx3/1000;
+  // target_pose2.position.y = yyy3/1000;
+  // target_pose2.position.z = zzz3/1000;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
 
   visual_tools.deleteAllMarkers();//清除所有标记
   visual_tools.publishText(text_pose, "robot", rvt::WHITE, rvt::XLARGE);//文本标记
   for(int k=0;k<num;k++)
   {
-  visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
-  for (std::size_t i = 0; i < waypoints[k].size(); ++i)
-    visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (i)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
-  visual_tools.trigger();
+    visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
+    for (std::size_t i = 0; i < waypoints[k].size(); ++i)
+      visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (k)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
+    visual_tools.trigger();
   }
   // while (ros::ok())
   // {
@@ -171,6 +211,7 @@ int main(int argc, char** argv)
   ros::shutdown();
   return 0;
 }
+
 /*
 BLACK 	
 BROWN 	

catkin_ws/更新记录.txt

@@ -1,49 +0,0 @@
-2024-7-31
-优化了轨迹规划：
-	1、做了关节限位 规划时不会出现大角度差姿态来回切换
-	2、做了轨迹间路径约束 移动时末端执行器不会走冗余路径
-	
-2024-8-5
-	1、
-	修改rvizload  通过py程序调起rviz
-	增加rviz启动等待功能
-	增加close_rviz 通过py接口关闭程序时顺带关闭rviz
-	（后期可以通过include限制在同一个终端  只是输出信息不方便查看 same_terminal_to_run.launch ）
-	2、修改点云加载 不再保存又立即读出
-	3、修改轨迹规划 重写之前做限制的函数 去除check 转而判断规划成功与否
-	4、增加ROS转py成员变量接口  包括：
-	{
-		焊接轨迹点坐标
-		焊接轨迹点隶属类型
-		焊接序号列表
-		焊接轨迹规划失败序号列表
-	}
-	
-	5、增加 不重新加载点云和焊接顺序姿态只重新规划机器人路径和运动学逆解 的接口
-	6、因为是多文件启动 修改程序逻辑防止规划器被2次启动
-	
-2024-8-7
-	
-	取消了焊缝剔除 通过延长焊丝坐标系的方式防碰撞（同时修改urdf和config 的end_effector_link）
-	优点：省去20s点云剔除时间 同时消除碰撞检测隐患
-	问题：有的焊缝要求焊丝必须比平时伸出的要长才能够到的话就会一直规划失败 因为路径点本就不可达 
-	
-	增加了轨迹可视化节点
-	
-	未能解决：
-	做初始化时间优化（本质是go函数第一次启动时间很长  搞不了 是系统封装的函数   而且好像是因为第一次要加载点云到碰撞检测)  实测时间与运行ROS机器性能有关
-	同时go的调用只能放在点云加载之后 否则没有对应的topic会报错 也导致无法进行预加载
-	
-	遇到新的问题 第六轴的翻转问题    
-	在焊接姿态处理中应该可以让焊缝焊接过程不进行翻转从而保证焊接的连续性（可能已经预留了末端执行器关节角度代码？  也大概率没有）  
-	那就得移动过程中来解决旋转的问题（有的焊接顺序就是避免不了翻转  焊缝姿态定了 那移动过程就会不可避免的要翻转 ） 而移动过程的翻转面临的最大问题就是 翻转过程不检测碰撞
-	
-2024-9-1
-	修复从起点规划到第一个失败后的重新设定约束逻辑问题
-	六轴翻转问题依旧没有解决  其问题会带来运动碰撞风险 不止是极限角度  也是之前轴翻转的根源
-2024-9-2
-	通过限制6轴自由度解决六轴翻转问题
-2024-9-5
-	隐患： 焊缝周围没有点云的的话会报错  （这个问题不是问题）
-		  T型焊缝未测试   对向焊缝失败处理未测试
-