荏原环境

ren_yuan/src/lst_robot_r/scripts/moveitServer2.py

@@ -1,18 +1,18 @@
 # 导入基本ros和moveit库
-from logging.handlers import RotatingFileHandler
 import os
 import moveit_msgs.msg
 import rospy, sys
 import moveit_commander      
 import moveit_msgs
+import tf.transformations
 
+from logging.handlers import RotatingFileHandler
 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
 
@@ -32,9 +32,9 @@ pi = np.pi
 class MoveIt_Control:
     # 初始化程序
     def __init__(self, is_use_gripper=False):
-        self.home_pose=[]
-        self.hf_num=0#焊缝计数 从0开始
-        self.hf_fail=[]
+        self.home_pose = []
+        self.hf_num = 0#焊缝计数 从0开始
+        self.hf_fail = []
         # Init ros configs
         moveit_commander.roscpp_initialize(sys.argv)
         self.robot = moveit_commander.RobotCommander()
@@ -61,12 +61,12 @@ class MoveIt_Control:
         # 机械臂初始姿态
         #self.move_j()
         self.go_home()
-        self.home_pose=self.get_now_pose()
+        self.home_pose = self.get_now_pose()
         #self.go_ready()
 
 
     #TODO plan_cartesian_path
-    def plan_cartesian_path_LLL(self,point_s,point_e,waypoints,trajectory,trajectory_with_type,v=1):
+    def plan_cartesian_path_LLL(self, point_s, point_e, waypoints, trajectory, trajectory_with_type, v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
@@ -74,36 +74,36 @@ class MoveIt_Control:
         #起点位置设置为规划组最后一个点
         start_pose = self.arm.get_current_pose(self.end_effector_link).pose
         wpose = deepcopy(start_pose)
-        waypoint=[]
+        waypoint = []
         #print(waypoint)
         if waypoints:
-            wpose.position.x=waypoints[-1].position.x
-            wpose.position.y=waypoints[-1].position.y
-            wpose.position.z=waypoints[-1].position.z
-            wpose.orientation.x=waypoints[-1].orientation.x
-            wpose.orientation.y=waypoints[-1].orientation.y
-            wpose.orientation.z=waypoints[-1].orientation.z
-            wpose.orientation.w=waypoints[-1].orientation.w
+            wpose.position.x = waypoints[-1].position.x
+            wpose.position.y = waypoints[-1].position.y
+            wpose.position.z = waypoints[-1].position.z
+            wpose.orientation.x = waypoints[-1].orientation.x
+            wpose.orientation.y = waypoints[-1].orientation.y
+            wpose.orientation.z = waypoints[-1].orientation.z
+            wpose.orientation.w = waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
-        wpose.position.x=point_s[0]
-        wpose.position.y=point_s[1]
-        wpose.position.z=point_s[2]
-        wpose.orientation.x=point_s[3]
-        wpose.orientation.y=point_s[4]
-        wpose.orientation.z=point_s[5]
-        wpose.orientation.w=point_s[6]
+        wpose.position.x = point_s[0]
+        wpose.position.y = point_s[1]
+        wpose.position.z = point_s[2]
+        wpose.orientation.x = point_s[3]
+        wpose.orientation.y = point_s[4]
+        wpose.orientation.z = point_s[5]
+        wpose.orientation.w = point_s[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
-        wpose.position.x=point_e[0]
-        wpose.position.y=point_e[1]
-        wpose.position.z=point_e[2]
-        wpose.orientation.x=point_e[3]
-        wpose.orientation.y=point_e[4]
-        wpose.orientation.z=point_e[5]
-        wpose.orientation.w=point_e[6]
+        wpose.position.x = point_e[0]
+        wpose.position.y = point_e[1]
+        wpose.position.z = point_e[2]
+        wpose.orientation.x = point_e[3]
+        wpose.orientation.y = point_e[4]
+        wpose.orientation.z = point_e[5]
+        wpose.orientation.w = point_e[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
@@ -120,8 +120,8 @@ class MoveIt_Control:
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
-            retimed_planed=self.retime_plan(trajj,v)
-            trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
+            retimed_planed = self.retime_plan(trajj, v)
+            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.append(trajj)
@@ -129,20 +129,15 @@ class MoveIt_Control:
             moveit_server.arm.execute(retimed_planed)
             rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
-            err=0
+            err = 0
         else:
-            err=1
+            err = 1
             rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))   
             
-            # rospy.loginfo("本次焊缝规划失败")
-            # points.pop()#将本条焊缝的起点从规划中删除
-            # trajectory.pop()
-            # trajectory_with_type.pop()
-            # self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
-        #self.hf_num=self.hf_num+1#焊缝计数
-        return waypoints,trajectory,trajectory_with_type,err
+        return waypoints, trajectory, trajectory_with_type, err
+    
     
-    def plan_cartesian_path_point(self,point_ss,point_ee,waypoints,trajectory,trajectory_with_type,v=1):
+    def plan_cartesian_path_point(self, point_ss, point_ee, waypoints, trajectory, trajectory_with_type, v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
@@ -150,36 +145,36 @@ class MoveIt_Control:
         #起点位置设置为规划组最后一个点
         start_pose = self.arm.get_current_pose(self.end_effector_link).pose
         wpose = deepcopy(start_pose)
-        waypoint=[]
+        waypoint = []
         #print(waypoint)
         if waypoints:
-            wpose.position.x=waypoints[-1].position.x
-            wpose.position.y=waypoints[-1].position.y
-            wpose.position.z=waypoints[-1].position.z
-            wpose.orientation.x=waypoints[-1].orientation.x
-            wpose.orientation.y=waypoints[-1].orientation.y
-            wpose.orientation.z=waypoints[-1].orientation.z
-            wpose.orientation.w=waypoints[-1].orientation.w
+            wpose.position.x = waypoints[-1].position.x
+            wpose.position.y = waypoints[-1].position.y
+            wpose.position.z = waypoints[-1].position.z
+            wpose.orientation.x = waypoints[-1].orientation.x
+            wpose.orientation.y = waypoints[-1].orientation.y
+            wpose.orientation.z = waypoints[-1].orientation.z
+            wpose.orientation.w = waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
-        wpose.position.x=point_ss[0]
-        wpose.position.y=point_ss[1]
-        wpose.position.z=point_ss[2]
-        wpose.orientation.x=point_ss[3]
-        wpose.orientation.y=point_ss[4]
-        wpose.orientation.z=point_ss[5]
-        wpose.orientation.w=point_ss[6]
+        wpose.position.x = point_ss[0]
+        wpose.position.y = point_ss[1]
+        wpose.position.z = point_ss[2]
+        wpose.orientation.x = point_ss[3]
+        wpose.orientation.y = point_ss[4]
+        wpose.orientation.z = point_ss[5]
+        wpose.orientation.w = point_ss[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
-        wpose.position.x=point_ee[0]
-        wpose.position.y=point_ee[1]
-        wpose.position.z=point_ee[2]
-        wpose.orientation.x=point_ee[3]
-        wpose.orientation.y=point_ee[4]
-        wpose.orientation.z=point_ee[5]
-        wpose.orientation.w=point_ee[6]
+        wpose.position.x = point_ee[0]
+        wpose.position.y = point_ee[1]
+        wpose.position.z = point_ee[2]
+        wpose.orientation.x = point_ee[3]
+        wpose.orientation.y = point_ee[4]
+        wpose.orientation.z = point_ee[5]
+        wpose.orientation.w = point_ee[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
@@ -196,30 +191,23 @@ class MoveIt_Control:
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
-            retimed_planed=self.retime_plan(trajj,v)
-            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"
+            retimed_planed = self.retime_plan(trajj, v)
+            trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
             trajectory.append(trajj)
             rospy.loginfo("开始执行")
             moveit_server.arm.execute(retimed_planed)
             rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
-            err=0
+            err = 0
         else:
-            err=1
+            err = 1
             rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))   
             
-            # rospy.loginfo("本次焊缝规划失败")
-            # points.pop()#将本条焊缝的起点从规划中删除
-            # trajectory.pop()
-            # trajectory_with_type.pop()
-            # self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
-        #self.hf_num=self.hf_num+1#焊缝计数
-        return waypoints,trajectory,trajectory_with_type,err
+        return waypoints, trajectory, trajectory_with_type, err
+     
      
     #TODO retime_plan
-    def retime_plan(self,traj,v):
+    def retime_plan(self, traj, v):
         current_state = self.arm.get_current_state()
         
         retimed_traj = self.arm.retime_trajectory(
@@ -230,10 +218,11 @@ class MoveIt_Control:
         
         return retimed_traj
     
+    
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
-        pose=[]
+        pose = []
         pose.append(current_pose.position.x)
         pose.append(current_pose.position.y)
         pose.append(current_pose.position.z)
@@ -245,130 +234,20 @@ class MoveIt_Control:
         # 打印位姿信息       
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
-    
-    def zhu_shi():#折叠注释代码用的
-        
-    # def create_path_constraints(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.16  
-    #     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
-
-    #     #构建 shape_msgs/SolidPrimitive 消息
-    #     bounding_volume = SolidPrimitive()
-    #     bounding_volume.type = SolidPrimitive.CYLINDER#圆柱
-    #     # bounding_volume.dimensions = [0.003,1]
-    #     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
-   
-
-    # 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.09    #初始允许半径 9cm
-    #     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_constraints(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_constraints(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)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
-        
-    #     #尝试规划次数
-    #     b = 4 #尝试规划5次  无约束5次
-    #     for i in range(b):
-    #         traj = self.arm.plan()#调用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("*******************")
-    #                 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("check failed! 移动轨迹无效")
-    #             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_constraints(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
-    #             else:
-    #                 path_constraints = self.create_path_constraints(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
-    #             self.arm.set_path_constraints(path_constraints)#设定约束
-    #             if(i>=(b-2)):
-    #                 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
-    
-        pass
 
 
-    def go_home(self,a=1,v=1):
+    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")
+        rospy.loginfo("get home_point")
         self.arm.go()
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
         
-    
 
-    def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
+    def go_home_justplan(self, trajectory, trajectory_with_type, 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)
@@ -381,36 +260,36 @@ class MoveIt_Control:
         trajj = traj[1]
         moveit_server.arm.execute(trajj)
         rospy.loginfo("go_home end")
-        traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
+        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
 
-    def path_planning(self,folder_path,gohome=True):
+    #TODO path_planning
+    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)
 
-        way_points,trajectory,trajectory_with_type = [],[],[]
+        way_points,trajectory,trajectory_with_type = [], [], []
         for i in range(len(all_data)):
-            rospy.loginfo("共读取到%d条焊缝，开始规划第%d条",len(all_data),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]
 
-            point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
-            point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
+            point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0], q1[1], q1[2], q1[3]]
+            point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0], q2[1], q2[2], q2[3]]
             
-            up_value=-0.1
-            point_up=[point11[0], point11[1], point11[2]-up_value, point11[3], point11[4],point11[5],point11[6]]
+            up_value = -0.1
+            point_up = [point11[0], point11[1], point11[2]-up_value, point11[3], point11[4], point11[5], point11[6]]
             # 点到点规划
-            way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_point(point_up,point11,way_points,trajectory,trajectory_with_type,v=speed_v)
+            way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_point(point_up, point11, way_points, trajectory, trajectory_with_type, v=speed_v)
             rospy.loginfo("末端执行器上移完毕")
             rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -418,11 +297,11 @@ class MoveIt_Control:
                 break
             
             # 焊缝规划
-            way_points,trajectory,trajectory_with_type,error = self.plan_cartesian_path_LLL(point11,point22,way_points,trajectory,trajectory_with_type,v=speed_v)
+            way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_LLL(point11, point22, way_points, trajectory, trajectory_with_type, v=speed_v)
             
-            rospy.loginfo("第%d条焊缝规划完毕",i+1)
+            rospy.loginfo("第%d条焊缝规划完毕", i+1)
             rospy.loginfo("*******************")
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -430,14 +309,14 @@ class MoveIt_Control:
                 break
             #向上移动末端执行器
             if i<len(all_data):
-                up_value=-0.1
-                point_up=[point22[0], point22[1], point22[2]-up_value, point22[3], point22[4],point22[5],point22[6]]
+                up_value = -0.1
+                point_up = [point22[0], point22[1], point22[2]-up_value, point22[3], point22[4], point22[5], point22[6]]
                 # 点到点规划
-                way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_point(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
+                way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_point(point22, point_up, way_points,trajectory,trajectory_with_type,v=speed_v)
                 rospy.loginfo("末端执行器上移完毕")
                 rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -447,11 +326,11 @@ class MoveIt_Control:
         #rospy.loginfo("*******************")
         if gohome:
             #trajectory,trajectory_with_type= self.move_p_flexible(trajectory,trajectory_with_type)
-            trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
+            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)
-        return trajectory,traj_merge,trajectory_with_type_merge
+        traj_merge = merge_trajectories(trajectory)
+        trajectory_with_type_merge = merge_trajectories_with_type(trajectory_with_type)
+        return trajectory, traj_merge, trajectory_with_type_merge
 
 ###########################################################class end#############################################################
 def process_welding_data(filename):
@@ -478,16 +357,17 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     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
+            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
 
+
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -547,11 +427,12 @@ def merge_trajectories_with_type(trajectory_with_type):
 
 
 class py_msgs():
-    fial=[]
-    shun_xv=[]
+    fial = []
+    shun_xv = []
     class point():
-        xyz_list=[]
-        type=[]
+        xyz_list = []
+        type = []
+
 
 #redis消息格式
 def ROS2PY_msgs(msgs, m_sr):
@@ -626,11 +507,11 @@ if __name__ =="__main__":
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-    speed_v=0.001
+    speed_v = 0.001
     # speed_v=get_user_input()
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
-    trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
+    trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
 
     #moveit_server.go_ready()       #合并后的轨迹也需要从ready点开始执行
 

ren_yuan/src/lst_robot_r/scripts/start.py

@@ -1,4 +1,5 @@
 #! /usr/bin/env python3
+import sys
 import rospy
 import os
 import command
@@ -7,6 +8,7 @@ import hjsx
 import threading
 import numpy as np
 import multiprocessing
+import actionlib_msgs.msg._GoalStatusArray
 from moveit_msgs.msg import PlanningScene
 from dynamic_reconfigure.client import Client
 from rospy.exceptions import ROSException
@@ -14,8 +16,6 @@ from std_srvs.srv import Empty
 from rospy.service import ServiceException
 from redis_publisher import Publisher
 
-import actionlib_msgs.msg._GoalStatusArray
-import sys
 
 def wait_for_topic(topic_name, message_type):
     try:
@@ -34,21 +34,21 @@ def clear_octomap():
         rospy.loginfo("Failed to call /clear_octomap service or it is not available. Skipping operation.")
 
 waited_once = False
-tim_list=[]
+tim_list = []
 
 if __name__ == "__main__":
     command.load_rviz()
     rospy.init_node("start_node")
 
     # 初始化各种类型参数
-    rospy.set_param("ping_sta",45)#用于判断平缝的角度
+    rospy.set_param("ping_sta", 45)#用于判断平缝的角度
     # rospy.set_param("sign_control",1)
-    rospy.set_param("sign_pointcloud",0)
-    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/123")
+    rospy.set_param("sign_pointcloud", 0)
+    rospy.set_param("folder_path", "/home/robot/ROS/ren_yuan/data/123")
 
 
     rospy.loginfo("等待rviz启动")
-    wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
+    wait_for_topic("/execute_trajectory/status", actionlib_msgs.msg.GoalStatusArray)
     
     publisher = Publisher()
     publisher.rds.set('sign_control', 0)
@@ -60,20 +60,11 @@ if __name__ == "__main__":
         if sign_control == "0":
             if not waited_once:
                 print("等待点云数据准备完成...")
-                # sys.stdin.flush()#清空键盘缓冲区
-                # aaa=input("请选择 1-2 默认为2: ")
-                # if aaa=="":
-                #     aaa='2'
-                # #aaa='2'
-                # if(aaa =='1' or aaa=='2'):
-                #     rospy.set_param("sign_control",aaa)
-                # else:
-                #     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
-                #     exit(0)
                 waited_once = True
+                
         elif sign_control == "1":
-            
             publisher.rds.set('sign_control', 0)
+            
             # 清除场景   点云计算并发布
             clear_octomap()
             process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
@@ -86,7 +77,7 @@ if __name__ == "__main__":
             rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
             rospy.loginfo(f"场景地图已加载完毕")
-            rospy.set_param("sign_pointcloud",1)
+            rospy.set_param("sign_pointcloud", 1)
 
             command.load_visual()
 
@@ -94,7 +85,6 @@ if __name__ == "__main__":
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
-
             while rospy.get_param("sign_control"):
                 pass
 
@@ -102,13 +92,13 @@ if __name__ == "__main__":
  
         elif sign_control == "2":
             publisher.rds.set('sign_control', 0)
+            
             hjsx.run()
             command.load_visual()
 
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
-
             while rospy.get_param("sign_control"):
                 pass
             waited_once = False