ren_yuan/ren_yuan_pointcloud/pcd2binary.py

@@ -0,0 +1,23 @@
+import open3d as o3d
+import numpy as np
+
+
+def pcd_to_binary(pcd_file, output_file):
+    pcd = o3d.io.read_point_cloud(pcd_file)
+    points = np.asarray(pcd.points)
+    binary_data = points[:, :3]
+    with open(output_file, 'wb') as f:
+        f.write(binary_data.tobytes())
+
+
+# def binary_to_nparray(binary_file, output_file):
+#     with open(binary_file, 'rb') as f:
+#         binary_data = f.read()
+#     # 将二进制数据转换为 NumPy 数组
+#     point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+#     np.savetxt(output_file, point_cloud)
+
+
+if __name__ == '__main__':
+    pcd_to_binary(r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/123.pcd', r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/pointcloud.txt')
+    #binary_to_nparray(r'./pointcloud.txt', r'./111123123123.txt')

ren_yuan/ren_yuan_pointcloud/说明.txt

@@ -0,0 +1,3 @@
+需要将相机排出的PCD点云手动裁切以及移动后 导出为txt 再打开txt然后导出为PCD 再用这里的代码转为2进制
+
+x往负方向1300   z 640   且绕z轴转一下（摆放问题）

ren_yuan/src/lst_robot_r/scripts/hjsx.py

@@ -1,7 +1,7 @@
 import numpy as np
 import os
 import rospy
-from math import acos, degrees,tan,atan,sqrt,sin,cos
+from math import acos, degrees,tan,atan,sqrt,sin,cos,radians
 from scipy.spatial.transform import Rotation as R
 
 #求向量的模
@@ -193,8 +193,9 @@ def run():
 
 
     faxiang=[middle2[0]-middle1[0],middle2[1]-middle1[1],middle2[2]-middle1[2]]
-    shuzhi=np.cross(faxiang,hanfeng1)#向量叉乘 
-
+    #shuzhi=np.cross(faxiang,hanfeng1)#向量叉乘 
+    shuzhi=[0,0,1]
+    
     faxiang/= np.linalg.norm(faxiang)  # 单位化
     shuzhi /= np.linalg.norm(shuzhi)  # 单位化
     
@@ -205,10 +206,8 @@ def run():
     # else:
     #     fangxiang=faxiang-shuzhi*5.6 #arctan(4)=76度
 
-    if(point1[0][0]<0 and point2[0][0]<0):
-        fangxiang=faxiang+shuzhi*5.6 #arctan(4)=76度
-    else:
-        fangxiang=faxiang-shuzhi*5.6 #arctan(4)=76度
+    ang_deg=30
+    fangxiang=-faxiang-shuzhi*tan(radians(90-ang_deg))#arctan(4)=76度
 
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数
@@ -220,10 +219,7 @@ def run():
     # else:
     #     fangxiang=-faxiang-shuzhi*5.6 #arctan(4)=76度
 
-    if(point1[0][0]<0 and point2[0][0]<0):
-        fangxiang=faxiang+shuzhi*5.6 #arctan(4)=76度
-    else:
-        fangxiang=-faxiang-shuzhi*5.6 #arctan(4)=76度
+    fangxiang=faxiang-shuzhi*tan(radians(90-ang_deg)) #arctan(4)=76度
         
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数

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
 
@@ -25,16 +25,16 @@ import traceback
 from lst_robot_r.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
-
+#from redis_publisher import Publisher
 
 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,58 +129,52 @@ 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
     
-    #TODO plan_cartesian_path
-    def plan_cartesian_path_CCC(self,point_s,point_m,point_e,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  # 已经尝试规划次数
-        
+
         #起点位置设置为规划组最后一个点
         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_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_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_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))
 
@@ -197,29 +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
+     
      
     #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,46 +260,48 @@ 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]]
-            way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_LLL(point_up,point11,way_points,trajectory,trajectory_with_type,v=speed_v)
+            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)
             rospy.loginfo("末端执行器上移完毕")
             rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
                 trajectory_with_type.clear()
                 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()
@@ -428,13 +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]]
-                way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
+                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)
                 rospy.loginfo("末端执行器上移完毕")
                 rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -444,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):
@@ -475,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
@@ -544,12 +427,14 @@ 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):
     for i in range(len(msgs.points)):
         py_msgs.point.xyz_list.append(msgs.points[i].positions)
@@ -615,19 +500,18 @@ def ROS2PY_msgs(msgs, m_sr):
         
 
 if __name__ =="__main__":
-    # from redis_publisher import Publisher  #注意 可能是史前redis 
-    # publisher = Publisher()
+    #publisher = Publisher()
 
     folder_path = rospy.get_param("folder_path")
     rospy.init_node('moveit_control_server', anonymous=False)   
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-    speed_v=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点开始执行
 
@@ -637,4 +521,8 @@ if __name__ =="__main__":
     # #ROS2_msgs_write(trajectory_with_type_merge,moveit_server)
     # rospy.loginfo("合并后轨迹执行完毕")
     
+    # message = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
+    # publisher.pub_plan_result(message)
+    # print("路径规划信息已发布....")
+
     rospy.set_param("sign_control",0)

ren_yuan/src/lst_robot_r/scripts/redis_publisher.py

@@ -1,47 +1,44 @@
 import time
 import redis
 
+def redis_init(max_retries=5, retry_delay=1):
+    pool = redis.ConnectionPool(host='192.168.100.133', port=6379, db=0)    #ip地址和端口号
+    retries = 0
+    while retries < max_retries:
+        try:
+            rds = redis.Redis(connection_pool=pool)
+            if rds.ping():
+                print("Connected to Redis successfully.")
+                return rds
+            else:
+                print("Redis 连接失败，正在尝试重新连接...")
+        except redis.ConnectionError as e:
+            print(f"Connection error (retry {retries + 1} of {max_retries}): {e}")
+        except Exception as e:
+            print(f"An error occurred (retry {retries + 1} of {max_retries}): {e}")
+
+        # 增加延迟时间，防止过于频繁的重试
+        time.sleep(retry_delay * (1 ** retries))
+        retries += 1
+
+    print("多次尝试重连失败,检查redis服务是否开启.")
+    return None
 
 class Publisher:
     def __init__(self):
-        print('redis初始化中..')
-        if not self.redis_init():
+        self.rds = redis_init()
+        if self.rds is None:
             raise Exception("Redis 初始化失败")
-        self.rds = None
-
-    def redis_init(self, max_retries=5, retry_delay=1):
-        pool = redis.ConnectionPool(host='localhost', port=6379, db=0)
-        retries = 0
-        while retries < max_retries:
-            try:
-                self.rds = redis.Redis(connection_pool=pool)
-                if self.rds.ping():
-                    print("Redis 连接成功.")
-                    return True
-                else:
-                    print("Redis 连接失败，正在尝试重新连接...")
-            except redis.ConnectionError as e:
-                print(f"Connection error (retry {retries + 1} of {max_retries}): {e}")
-            except Exception as e:
-                print(f"An error occurred (retry {retries + 1} of {max_retries}): {e}")
-
-            # 增加延迟时间，防止过于频繁的重试
-            time.sleep(retry_delay * (1 ** retries))
-            retries += 1
-
-        print("多次尝试重连失败,检查redis服务是否开启.")
-        return False
 
     def pub_plan_result(self, result_dict):
         """
         发布路径规划结果
         """
         if self.rds is not None:
-            self.rds.publish('plan_result', result_dict)
+            self.rds.publish('ros_plan_result', result_dict)
         else:
             self.redis_init()
-            self.rds.publish('plan_result', result_dict)
-
+            self.rds.publish('ros_plan_result', result_dict)
 
 if __name__ == '__main__':
     Publisher()

ren_yuan/src/lst_robot_r/scripts/start.py

@@ -16,6 +16,8 @@ from rospy.service import ServiceException
 import actionlib_msgs.msg._GoalStatusArray
 import sys
 
+#from redis_publisher import Publisher
+
 def wait_for_topic(topic_name, message_type):
     try:
         message = rospy.wait_for_message(topic_name, message_type, timeout=None)
@@ -41,15 +43,19 @@ if __name__ == "__main__":
 
     # 初始化各种类型参数
     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.loginfo("等待rviz启动")
     wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
     
+    rospy.set_param("sign_control",1)
+    # publisher = Publisher()
+    # publisher.rds.set('sign_control', 0)
+
     while not rospy.is_shutdown():
+        #sign_control = publisher.rds.get('sign_control').decode('ascii')
         sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
             if not waited_once:
@@ -93,6 +99,9 @@ if __name__ == "__main__":
             waited_once = False      #运行结束，重置参数
  
         elif sign_control == "2":
+
+            #publisher.rds.set('sign_control', 0)
+
             hjsx.run()
             command.load_visual()
 
@@ -104,5 +113,8 @@ if __name__ == "__main__":
                 pass
             waited_once = False
 
-    
+        # elif sign_control == "3":
+        #     rospy.loginfo("正在关闭规划器....")
+        #     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
+        #     sys.exit(0)
     

ren_yuan/src/yunhua_1006a_moveit_config/config/fake_controllers.yaml

@@ -10,4 +10,4 @@ controller_list:
       - joint6
 initial:  # Define initial robot poses per group
   - group: manipulator
-    pose: zero
+    pose: home

ren_yuan/src/yunhua_1006a_moveit_config/config/yunhua_ryhj.srdf

@@ -13,22 +13,23 @@
         <chain base_link="base_link" tip_link="link_end"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="zero" group="manipulator">
+    <group_state name="home" group="manipulator">
         <joint name="joint1" value="0"/>
         <joint name="joint2" value="0"/>
         <joint name="joint3" value="0"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="0"/>
+        <joint name="joint5" value="1.57"/>
         <joint name="joint6" value="0"/>
     </group_state>
-    <group_state name="home" group="manipulator">
+    <group_state name="zero" group="manipulator">
         <joint name="joint1" value="0"/>
         <joint name="joint2" value="0"/>
         <joint name="joint3" value="0"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="1.57"/>
+        <joint name="joint5" value="0"/>
         <joint name="joint6" value="0"/>
     </group_state>
+
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link2" reason="Never"/>

ren_yuan/src/yunhua_1006a_moveit_config/launch/moveit.rviz

@@ -343,7 +343,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.6265817284584045
+      Distance: 2.882205009460449
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -351,17 +351,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.2947564125061035
-        Y: 0.001691619516350329
-        Z: 0.49094098806381226
+        X: 1.2886135578155518
+        Y: 0.007511036470532417
+        Z: 0.44920703768730164
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.07499924302101135
+      Pitch: 0.15000008046627045
       Target Frame: base_link
-      Yaw: 0.02673092484474182
+      Yaw: 0.18173374235630035
     Saved: ~
 Window Geometry:
   Displays: