2024-10-11

ren_yuan/cmd.txt

@@ -1 +0,0 @@
-roslaunch lst_robot_r start.launch

ren_yuan/src/a9/urdf/a9.urdf

@@ -105,8 +105,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-2.965"
+      upper="2.965"
       effort="0"
       velocity="0" />
   </joint>
@@ -163,8 +163,8 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-2.27"
+      upper="2.27"
       effort="0"
       velocity="0" />
   </joint>
@@ -221,8 +221,8 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-4.0"
+      upper="4.0"
       effort="0"
       velocity="0" />
   </joint>
@@ -279,8 +279,8 @@
     <axis
       xyz="0.86603 0 0.5" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-3.49"
+      upper="3.49"
       effort="0"
       velocity="0" />
   </joint>
@@ -337,8 +337,8 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-2.49"
+      upper="2.49"
       effort="0"
       velocity="0" />
   </joint>
@@ -395,8 +395,8 @@
     <axis
       xyz="0.49999 0 -0.86603" />
     <limit
-      lower="-3.8"
-      upper="3.8"
+      lower="-7.855"
+      upper="7.855"
       effort="0"
       velocity="0" />
   </joint>

ren_yuan/src/a9/urdf/a9（复件）.urdf

@@ -0,0 +1,465 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="a9">
+  <link name="world" />
+  
+  <joint name="world_joint" type="fixed">
+    <parent link="world" />
+    <child link = "base_link" />
+    <origin xyz="0.0 0.0 2.17" rpy="0.0 3.14 0.0" />
+  </joint>
+  
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282"
+        rpy="0 0 0" />
+      <mass
+        value="0.0148298653353077" />
+      <inertia
+        ixx="2.02744416702812E-06"
+        ixy="-1.54215522606031E-15"
+        ixz="-3.37592192750916E-14"
+        iyy="1.40267326461625E-06"
+        iyz="-2.76126618174793E-15"
+        izz="1.40267288470545E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="Link_1">
+    <inertial>
+      <origin
+        xyz="0.802922442229256 0.00984539192698494 1.19598646678643"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.19323050511757"
+        ixy="0.0386299594946521"
+        ixz="-0.388616965148905"
+        iyy="1.42567974975729"
+        iyz="0.0128991341474255"
+        izz="1.24513273855516" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.278"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="Link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_2">
+    <inertial>
+      <origin
+        xyz="0.117746157865896 0.0426453911640452 1.20871486266949"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.723493788556202"
+        ixy="0.0285843029943905"
+        ixz="-0.653933580723206"
+        iyy="1.42567974975729"
+        iyz="0.0290103266895879"
+        izz="0.714869455116528" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0.15 -0.0328 0.172"
+      rpy="0 0.47052 0" />
+    <parent
+      link="Link_1" />
+    <child
+      link="Link_2" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_3">
+    <inertial>
+      <origin
+        xyz="0.00164806841162357 0.0743961632896021 -0.0125870623969895"
+        rpy="0 0 0" />
+      <mass
+        value="1.10722558532838" />
+      <inertia
+        ixx="0.00281804497282514"
+        ixy="1.0820613146504E-05"
+        ixz="-2.30905562044302E-05"
+        iyy="0.00127732014523064"
+        iyz="5.62797076557556E-06"
+        izz="0.0028367630953507" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="-0.59925 0 0.50037"
+      rpy="0 -0.28892 0" />
+    <parent
+      link="Link_2" />
+    <child
+      link="Link_3" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_4">
+    <inertial>
+      <origin
+        xyz="0.751501000756241 -0.00505225715113682 0.690060032582326"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.419099372956719"
+        ixy="-0.0348631653478693"
+        ixz="-0.581012917159635"
+        iyy="1.42564974048808"
+        iyz="-0.0218580250133829"
+        izz="1.01929387998522" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="-0.084223 0.0347 0.16884"
+      rpy="-3.119 -1.0471 -0.013042" />
+    <parent
+      link="Link_3" />
+    <child
+      link="Link_4" />
+    <axis
+      xyz="0.86603 0 0.5" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_5">
+    <inertial>
+      <origin
+        xyz="0.206551018322306 -0.00505225715114058 -0.104834036546721"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.745717188604768"
+        ixy="0.0281122476978264"
+        ixz="-0.653399595391099"
+        iyy="1.42564974048808"
+        iyz="0.030048545471202"
+        izz="0.69267606433717" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0.92436 0 0.53588"
+      rpy="3.1416 -0.25867 3.1416" />
+    <parent
+      link="Link_4" />
+    <child
+      link="Link_5" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_6">
+    <inertial>
+      <origin
+        xyz="0.12280626304832 0.10284172733946 -0.0673247079199191"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="1.25065484857243"
+        ixy="-0.370837141231599"
+        ixz="-0.214061773475593"
+        iyy="0.581725625286335"
+        iyz="-0.544088624970368"
+        izz="1.03166251957125" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0.037697 -0.0006997 -0.064094"
+      rpy="0.44032 0.19669 -0.83053" />
+    <parent
+      link="Link_5" />
+    <child
+      link="Link_6" />
+    <axis
+      xyz="0.49999 0 -0.86603" />
+    <limit
+      lower="-3.8"
+      upper="3.8"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_7">
+    <inertial>
+      <origin
+        xyz="0.0296425829147993 -6.47728578861279E-06 0.162281442392356"
+        rpy="0 0 0" />
+      <mass
+        value="7.07143232149748" />
+      <inertia
+        ixx="0.0103947066541271"
+        ixy="8.00167786886164E-07"
+        ixz="0.00131122028195651"
+        iyy="0.0112643862205598"
+        iyz="-2.38846633634112E-06"
+        izz="0.0065729384023248" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="-2.6972 0.28661 -2.1066" />
+    <parent
+      link="Link_6" />
+    <child
+      link="Link_7" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link name="end_link_now" />
+  
+  <joint name="end_joint" type="fixed">
+    <parent link="Link_7" />
+    <child link = "end_link_now" />
+    <origin xyz="-0.045 0.0 0.4" rpy="0.0 0.4 3.14" />
+  </joint>
+  
+</robot>

ren_yuan/src/lst_robot_r/scripts/dycl_0506.py

@@ -82,7 +82,7 @@ def load_point_cloud_from_binary_txt(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)
+pcd = pcd.voxel_down_sample(voxel_size = 4)
 
 ################################去掉焊缝周围的点云#################################
 # 计算焊缝向量
@@ -112,7 +112,7 @@ ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)
 def build_pointcloud2_msg(points):
     msg = PointCloud2()
     msg.header.stamp = rospy.Time(0)
-    msg.header.frame_id = "base_link"
+    msg.header.frame_id = "world"#base_link
 
     if len(points.shape) == 3:
         msg.height = points.shape[1]

ren_yuan/src/lst_robot_r/scripts/hanqiangpose.py

@@ -30,7 +30,7 @@ def get_hanfeng(file_path):
             point1_str = points_str[0].split(',')
             point2_str = points_str[1].split(',')
             
-            # 转换字符串为浮点数列表，构造三维点
+            # 转换字符串为浮点数列表，构造三维点，使用列表推导式
             point1 = [float(coord) for coord in point1_str]
             point2 = [float(coord) for coord in point2_str]
 
@@ -67,12 +67,12 @@ def run():
     # 计算位姿
     for i in range(len(hanfeng)): 
         if(calculate_angle_with_xy_plane(start_points[i], end_points[i])<45):#平缝
-            q1 = rpy2quaternion(0,0,np.pi/2)
-            q2 = rpy2quaternion(0,0,np.pi/2)
+            q1 = rpy2quaternion(0,0,3*np.pi/4)    #ping
+            q2 = rpy2quaternion(0,0,3*np.pi/4)
             result.append(("s", start_points[i], end_points[i], q1, q2))
         else:
-            q1 = rpy2quaternion(0,np.pi/2,0)
-            q2 = rpy2quaternion(0,np.pi/2,0)
+            q1 = rpy2quaternion(0,np.pi/4,0)      #shu
+            q2 = rpy2quaternion(0,np.pi/4,0)
             result.append(("s", start_points[i], end_points[i], q1, q2))
 
 
@@ -86,4 +86,5 @@ def run():
     rospy.loginfo("Welding Angle calculation completed")
 if __name__ == '__main__':     
     
-    run()
+    run()
+    

ren_yuan/src/lst_robot_r/scripts/hjsx.py

@@ -2,6 +2,7 @@ import numpy as np
 import os
 import rospy
 from math import acos, degrees
+
 def calculate_distance(point1, point2):
     return np.linalg.norm(np.array(point1) - np.array(point2))
 
@@ -49,7 +50,7 @@ def get_hanfeng(file_path):
 
         angle = calculate_angle_with_xy_plane(point1,point2)
         
-        if abs(angle) < 30:
+        if abs(angle) < 30:     #与xy平面夹角小于30度
             data_ping.append([point1, point2, flag_sequence])
         else:
             data_shu.append([*sorted([point1, point2], key=lambda p: p[2]), flag_sequence]) 
@@ -84,8 +85,8 @@ def sort_welds_by_distance_ping(data_ping, reference_point):
         data_with_distances.sort(key=lambda x: x[0])
 
         # 初始化结果和当前终点
-        sorted_data_ping.append(data_with_distances.pop(0)[-3:])
-        reference_point = sorted_data_ping[-1][1]
+        sorted_data_ping.append(data_with_distances.pop(0)[-3:])   
+        reference_point = sorted_data_ping[-1][1]                   
         data_ping = [[start, end,flag_sequence] for distances1,distances2, start,end,flag_sequence in data_with_distances]
     return sorted_data_ping
 def sort_welds_by_distance_shu(data_shu, reference_point):

ren_yuan/src/lst_robot_r/scripts/moveitServer2.py

@@ -22,7 +22,7 @@ import tf
 from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
 import math
 import traceback
-from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
+from lst_robot_r.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
 from decorator_time import decorator_time
@@ -63,18 +63,19 @@ class MoveIt_Control:
         #self.move_j()
         self.go_home()
         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,a=1,v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
 
         #起点位置设置为规划组最后一个点
-        pose = self.arm.get_current_pose(self.end_effector_link).pose
-        wpose = deepcopy(pose)
+        start_pose = self.arm.get_current_pose(self.end_effector_link).pose
+        wpose = deepcopy(start_pose)
         waypoint=[]
-        print(waypoint)
+        #print(waypoint)
         if waypoints:
             wpose.position.x=waypoints[-1].position.x
             wpose.position.y=waypoints[-1].position.y
@@ -84,16 +85,6 @@ class MoveIt_Control:
             wpose.orientation.z=waypoints[-1].orientation.z
             wpose.orientation.w=waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
-        # else:
-        #     wpose.position.x=self.home_pose[0]
-        #     wpose.position.y=self.home_pose[1]
-        #     wpose.position.z=self.home_pose[2]
-        #     wpose.orientation.x=self.home_pose[3]
-        #     wpose.orientation.y=self.home_pose[4]
-        #     wpose.orientation.z=self.home_pose[5]
-        #     wpose.orientation.w=self.home_pose[6]
-        #     waypoint.append(deepcopy(wpose))
-            #waypoints.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
         wpose.position.x=point_s[0]
@@ -129,11 +120,14 @@ class MoveIt_Control:
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
+            retimed_planed=self.retime_plan(trajj)
             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)
-            moveit_server.arm.execute(trajj)
+            rospy.loginfo("开始执行")
+            moveit_server.arm.execute(retimed_planed)
+            rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
         else:
             rospy.loginfo(
@@ -147,7 +141,18 @@ class MoveIt_Control:
         #self.hf_num=self.hf_num+1#焊缝计数
         return waypoints,trajectory,trajectory_with_type
     
+    def retime_plan(self,traj):
+        current_state = self.arm.get_current_state()
+        
+        retimed_traj = self.arm.retime_trajectory(
+            current_state,
+            traj,
+            velocity_scaling_factor=0.5,
+            acceleration_scaling_factor=1)
+        
+        return retimed_traj
 
+    
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -272,8 +277,6 @@ class MoveIt_Control:
         return points,trajectory,trajectory_with_type,er
     
 
-
-
     #TODO go_home
     @decorator_time  
     def go_home(self,a=1,v=1):
@@ -285,9 +288,20 @@ class MoveIt_Control:
         self.arm.go()
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
+        
+    def go_ready(self,a=1,v=1):
+        rospy.loginfo("go_ready start")
+        self.arm.set_max_acceleration_scaling_factor(a)
+        self.arm.set_max_velocity_scaling_factor(v)
+        self.arm.set_named_target('ready')
+        rospy.loginfo("get_ready end")
+        self.arm.go()
+        rospy.loginfo("go_ready end")
     
     #TODO go_home_justplan
+    @decorator_time
     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)
         state = self.arm.get_current_state()
@@ -296,6 +310,8 @@ class MoveIt_Control:
         self.arm.set_named_target('home')
         traj = self.arm.plan()
         trajj = traj[1]
+        moveit_server.arm.execute(trajj)
+        rospy.loginfo("go_home end")
         traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
         trajectory.append(trajj)
         trajectory_with_type.append(traj_with_type)
@@ -321,13 +337,14 @@ class MoveIt_Control:
 
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             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)
+        rospy.loginfo("All of The trajectory Plan successfully")
+        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)
 
         traj_merge= merge_trajectories(trajectory)
         trajectory_with_type_merge= merge_trajectories_with_type(trajectory_with_type)
-        rospy.loginfo("All of The trajectory Plan successfully")
         return trajectory,traj_merge,trajectory_with_type_merge
 
 ###########################################################class end#############################################################
@@ -365,7 +382,6 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
-
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -482,12 +498,9 @@ def get_user_input():
         #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
         return vv
     except Exception as e:
-        rospy.logerr(f"发生错误：{e}")
+        rospy.logerr(f"发生错误：{e}") 
         
 
-            
-
-
 if __name__ =="__main__":
     # from redis_publisher import Publisher
     # publisher = Publisher()
@@ -502,11 +515,10 @@ if __name__ =="__main__":
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
-    #ROS2_msgs(trajectory_with_type_merge,moveit_server)
-    #write_ee(trajectory_with_type_merge, moveit_server)
-
 
-    # moveit_server.go_home()
-    # moveit_server.arm.execute(trajectory_merge)
+    #moveit_server.go_ready()
+    rospy.loginfo("开始执行合并后轨迹")
+    moveit_server.arm.execute(trajectory_merge)
+    rospy.loginfo("合并后轨迹执行完毕")
     
     rospy.set_param("sign_control",0)

ren_yuan/src/lst_robot_r/scripts/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/tong/renyuan_pointcloud/3/26_LazerData_Hi229204_2.pcd', r'/home/tong/renyuan_pointcloud/3/pointcloud.txt')
+    #binary_to_nparray(r'./pointcloud.txt', r'./111123123123.txt')

ren_yuan/src/lst_robot_r/scripts/rotation_pcd.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import rospy
+import pcl
+import numpy as np
+
+def main():
+    # 初始化ROS节点
+    rospy.init_node('transform_pcd_node', anonymous=True)
+
+    # 定义点云地图的路径和文件名
+    pcd_path = "/home/tong/renyuan_pointcloud/2/26_LazerData_Hi229204_1.pcd"
+
+    # 读取点云地图
+    cloud = pcl.load(pcd_path)
+
+    # 手动变换点云
+    transformed_points = cloud.to_array()
+    
+    #定义位移向量(沿Z轴方向移动一个单位长度)
+    
+    print("Transformed Points (before translation):", transformed_points)
+    
+    translation_vector = np.array([0, 0, 1000], dtype=np.float32)
+    transformed_points += translation_vector
+    
+    print("Transformed Points (after translation):", transformed_points)
+    
+    # 创建新的点云对象
+    transformed_cloud = pcl.PointCloud()
+    transformed_cloud.from_array(transformed_points.astype(np.float32))
+
+    # 保存旋转后的点云地图到PCD文件中
+    pcl.save(transformed_cloud, "/home/tong/renyuan_pointcloud/2/transformed_point_cloud.pcd")
+
+if __name__ == "__main__":
+    main()
+

ren_yuan/src/lst_robot_r/scripts/start.py

@@ -47,7 +47,8 @@ if __name__ == "__main__":
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
 
-    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/3")
+    #rospy.set_param("folder_path","/home/tong/moveit_ws/data/3")
+    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/2")
 
    
     rospy.loginfo("等待rviz启动")
@@ -78,27 +79,31 @@ if __name__ == "__main__":
             # 清除场景
             clear_octomap()
             #点云计算并发布
-            # process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            # process.start()
-            #计算焊接顺序和焊接姿态
-            hjsx.run()
-            hanqiangpose.run()
-            time.sleep(10)
-            command.load_visual()
-            # 等待 /move_group/monitored_planning_scene 话题发布消息
-            # rospy.loginfo("等待场景地图加载完毕...")
-            # wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            # rospy.loginfo(f"场景地图已加载完毕")
+            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            process.start()
+
+            # input("请在rviz中查看点云，然后按回车键继续...")
+            # #计算焊接顺序和焊接姿态
+            # hjsx.run()
+            # hanqiangpose.run()
+            # time.sleep(10)
+            # command.load_visual()
+            # # 等待 /move_group/monitored_planning_scene 话题发布消息
+            rospy.loginfo("等待场景地图加载完毕...")
+            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
-            #rospy.loginfo("终止点云发布，关闭发布窗口")
+            # #rospy.loginfo("终止点云发布，关闭发布窗口")
             
-            rospy.loginfo("运行moveitserver2.py")
-            process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
-            process.start()
+            # rospy.loginfo("运行moveitserver2.py")
+            # process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
+            # process.start()
 
-            while rospy.get_param("sign_control"):
-                pass
-            #运行结束，重置参数
+            # while rospy.get_param("sign_control"):
+            #     pass
+            # #运行结束，重置参数
+
+            rospy.set_param("sign_control",0)#记得删除！！！！！！！！！！！！！！！！
             waited_once = False
  
         elif sign_control == "2":

ren_yuan/src/robot_config/config/a9.srdf

@@ -13,6 +13,14 @@
         <chain base_link="world" tip_link="end_link_now"/>
     </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="home" group="manipulator">
+        <joint name="joint_1" value="0"/>
+        <joint name="joint_2" value="-1.1311"/>
+        <joint name="joint_3" value="1.459"/>
+        <joint name="joint_4" value="-3.158"/>
+        <joint name="joint_5" value="-0.258"/>
+        <joint name="joint_6" value="-0.022"/>
+    </group_state>
     <group_state name="zero" group="manipulator">
         <joint name="joint_1" value="0"/>
         <joint name="joint_2" value="0"/>
@@ -21,13 +29,13 @@
         <joint name="joint_5" value="0"/>
         <joint name="joint_6" value="0"/>
     </group_state>
-    <group_state name="home" group="manipulator">
+    <group_state name="ready" group="manipulator">
         <joint name="joint_1" value="0"/>
-        <joint name="joint_2" value="-1.1311"/>
+        <joint name="joint_2" value="-1.278"/>
         <joint name="joint_3" value="1.459"/>
         <joint name="joint_4" value="0"/>
-        <joint name="joint_5" value="2.8033"/>
-        <joint name="joint_6" value="2.6393"/>
+        <joint name="joint_5" value="2.490"/>
+        <joint name="joint_6" value="3.102"/>
     </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="Link_1" link2="Link_2" reason="Adjacent"/>

ren_yuan/src/robot_config/config/chomp_planning.yaml

@@ -16,3 +16,4 @@ collision_threshold: 0.07
 use_stochastic_descent: true
 enable_failure_recovery: false
 max_recovery_attempts: 5
+trajectory_initialization_method: "fillTrajectory"

ren_yuan/src/robot_config/config/fake_controllers.yaml

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

ren_yuan/src/robot_config/config/sensors_3d.yaml

@@ -6,4 +6,5 @@ sensors:
     padding_scale: 1.0
     point_cloud_topic: /pointcloud/output
     point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+

ren_yuan/src/robot_config/launch/moveit.rviz

@@ -4,12 +4,11 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /Grid1
-        - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
+        - /MotionPlanning1/Planning Request1
+        - /RobotModel1/Links1
       Splitter Ratio: 0.5
-    Tree Height: 808
+    Tree Height: 370
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -40,7 +39,7 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 10
-      Reference Frame: world
+      Reference Frame: base_link
       Value: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
@@ -290,16 +289,7 @@ Visualization Manager:
       Marker Topic: rviz_visual_tools
       Name: MarkerArray
       Namespaces:
-        Path: true
-        Text: true
-        point0  0.983265  0.126875  1.478115: true
-        point0  1.125382  0.127069  1.480059: true
-        point1  0.983994  -0.261691  1.478523: true
-        point1  1.125382  -0.262691  1.480059: true
-        point2  0.983294  -0.262191  1.663923: true
-        point2  0.984994  -0.263491  1.477923: true
-        point3  0.980265  0.126875  1.478115: true
-        point3  0.983365  0.126875  1.668115: true
+        {}
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -328,11 +318,39 @@ Visualization Manager:
         {}
       Update Interval: 0
       Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pointcloud/output
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: world
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -344,7 +362,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.1756603717803955
+      Distance: 2.2325363159179688
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -352,17 +370,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.8280303478240967
-        Y: -0.09661253541707993
-        Z: 0.6303508281707764
+        X: 0.9615159630775452
+        Y: -0.2659122049808502
+        Z: -1.0034211874008179
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.15500007569789886
+      Pitch: 0.5497965812683105
       Target Frame: world
-      Yaw: 1.8181167840957642
+      Yaw: 5.431985855102539
     Saved: ~
 Window Geometry:
   Displays:
@@ -376,7 +394,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001ee0000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d00000203000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000246000001b00000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1920

ren_yuan/src/robot_config/launch/sensor_manager.launch.xml

@@ -4,14 +4,18 @@
 
   <!-- Params for 3D sensors config -->
   <rosparam command="load" file="$(find robot_config)/config/sensors_3d.yaml" />
-
+  <!-- <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/sensors_3d2.yaml" /> -->
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.025" />
-  <param name="max_range" type="double" value="5.0" />
+  <param name="octomap_resolution" type="double" value="0.004" />
+  <param name="max_range" type="double" value="5.0" /> 
 
+  
+  
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="a9" />
   <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
 
+
+
 </launch>

ren_yuan/src/visual/CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-  lstrobot_moveit_config_0605
+  robot_config
 )
 
 catkin_package(