2024-8-12

catkin_ws/src/lstrobot_moveit_config_0605/config/kinematics.yaml

@@ -4,4 +4,4 @@ manipulator:
   kinematics_solver_timeout: 0.005
   goal_joint_tolerance: 0.0001
   goal_position_tolerance: 0.0001
-  goal_orientation_tolerance: 0.001
+  goal_orientation_tolerance: 0.001

catkin_ws/src/lstrobot_moveit_config_0605/config/mr12urdf20240605.srdf

@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="link_end"/>
+        <chain base_link="base_link" tip_link="link_end_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">

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -4,9 +4,12 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /MotionPlanning1
-      Splitter Ratio: 0.5
-    Tree Height: 557
+        - /MotionPlanning1/Scene Geometry1
+        - /MotionPlanning1/Planning Request1
+        - /RobotModel1/Links1
+        - /Trajectory1/Links1
+      Splitter Ratio: 0.4285714328289032
+    Tree Height: 386
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -42,14 +45,14 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      JointsTab_Use_Radians: false
+      JointsTab_Use_Radians: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false
       MoveIt_Allow_Sensor_Positioning: false
       MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
+      MoveIt_Planning_Time: 25
       MoveIt_Use_Cartesian_Path: false
       MoveIt_Use_Constraint_Aware_IK: false
       MoveIt_Workspace:
@@ -71,51 +74,7 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link_end:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          link_flp:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: true
+        Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: false
@@ -161,61 +120,17 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          Link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link_end:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          link_flp:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: false
       Value: true
       Velocity_Scaling_Factor: 0.1
-    - Alpha: 1
+    - Alpha: 0.5
       Class: rviz/RobotModel
       Collision Enabled: false
       Enabled: true
       Links:
-        All Links Enabled: true
+        All Links Enabled: false
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
@@ -224,27 +139,27 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         Link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         Link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         Link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         Link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         Link6:
           Alpha: 1
           Show Axes: false
@@ -254,16 +169,20 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
         link_end:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        link_end_now:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         link_flp:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+          Value: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -298,6 +217,63 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 0.029999999329447746
+      Name: Axes
+      Radius: 0.004000000189989805
+      Reference Frame: link_end_now
+      Show Trail: false
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: true
+      Enabled: false
+      Interrupt Display: false
+      Links:
+        All Links Enabled: false
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 1
+      Robot Color: 115; 210; 22
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: false
+      Show Trail: true
+      State Display Time: 3x
+      Trail Step Size: 1
+      Trajectory Topic: move_group/display_planned_path
+      Use Sim Time: false
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -314,7 +290,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.112061977386475
+      Distance: 0.48905709385871887
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -322,33 +298,35 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.7538955211639404
-        Y: 0.3512139916419983
-        Z: 0.6472357511520386
+        X: 1.0607073307037354
+        Y: -0.08724305778741837
+        Z: 0.4003370702266693
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5000001788139343
+      Pitch: -0.9597961902618408
       Target Frame: base_link
-      Yaw: 4.689955711364746
+      Yaw: 6.070621013641357
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 792
+    collapsed: false
+  Height: 1043
   Help:
     collapsed: false
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002befc0200000007fb000000100044006900730070006c006100790073000000003d000002be000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015b000001a00000017d00ffffff000004d0000002be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000009fb000000100044006900730070006c006100790073010000003d00000213000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000256000001a00000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff000000160000001600000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Trajectory - Trajectory Slider:
+    collapsed: false
   Views:
     collapsed: false
-  Width: 1232
-  X: 485
-  Y: 119
+  Width: 1920
+  X: 0
+  Y: 0

catkin_ws/src/lstrobot_planning/launch/test.launch

@@ -1,5 +0,0 @@
-<launch>
-  <node pkg="lstrobot_planning" name="test" type="11.py" required="true" output="screen">
-  </node>
-
-</launch>

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

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

catkin_ws/src/lstrobot_planning/scripts/11.py

@@ -1,23 +0,0 @@
-#! /usr/bin/env python3
-import rospy
-import os
-from std_srvs.srv import Empty
-from subprocess import Popen
-
-
-if __name__ == '__main__':
-    
-    rospy.init_node("test")
-    rospy.set_param("sign_control",0)
-    while not rospy.is_shutdown():
-        if(rospy.get_param("sign_control")==0):
-            aaa=input("请选择 1-2: ")
-            if(aaa =='1' or aaa=='2'):
-                rospy.set_param("sign_control",aaa)
-            else:
-                exit(0)
-            
-   
-   
-
-     

catkin_ws/src/lstrobot_planning/scripts/command.py

@@ -20,7 +20,10 @@ def close_rviz(terminal_name):
             os.kill(int(pid), signal.SIGTERM)
         except ProcessLookupError:
             pass  # 进程可能已经不存在
-
+        
+def load_visual():
+    command = "rosrun visual visual_node"
+    subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def load_rviz():
     command = "roslaunch lstrobot_moveit_config_0605 demo.launch"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])

catkin_ws/src/lstrobot_planning/scripts/dycl_0506.py

@@ -89,10 +89,19 @@ pcd = pcd.voxel_down_sample(voxel_size = 4)
 start_points,end_points,vectors = np.array(read_and_calculate_vectors(file_path_points))
 
 data = np.asarray(pcd.points)
-culling_radius2 = float(rospy.get_param('culling_radius'))
-data_retained2,data_culled2 =cull_pointcloud(data,culling_radius2)
 
-data_scaled2 = np.array(data_retained2) / 1000
+
+
+# culling_radius2 = float(rospy.get_param('culling_radius'))
+# data_retained2,data_culled2 =cull_pointcloud(data,culling_radius2)
+# data_scaled2 = np.array(data_retained2) / 1000
+
+
+data_scaled2 = np.array(data) / 1000
+
+
+
+
 
 ptCloud_scaled1 = o3d.geometry.PointCloud()
 

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -40,7 +40,7 @@ class MoveIt_Control:
         self.arm.set_goal_joint_tolerance(0.0001)
         self.arm.set_goal_position_tolerance(0.0005)
         self.arm.set_goal_orientation_tolerance(0.1)
-        self.arm.set_planner_id("RRTConnet")#不知道能不能用
+        #self.arm.set_planner_id("RRTConnet")#不知道能不能用
 
         self.end_effector_link = self.arm.get_end_effector_link()
         # 设置机械臂基座的参考系
@@ -48,7 +48,7 @@ class MoveIt_Control:
         self.arm.set_pose_reference_frame(self.reference_frame)
 
         # # 设置最大规划时间和是否允许重新规划
-        self.arm.set_planning_time(10)
+        self.arm.set_planning_time(10.0)
         self.arm.allow_replanning(True)
 
         self.arm.set_max_acceleration_scaling_factor(1)
@@ -70,7 +70,7 @@ class MoveIt_Control:
         self.arm.set_joint_value_target(joint_configuration)
         rospy.loginfo("move_j:"+str(joint_configuration))
         self.arm.go()
-        rospy.sleep(1)
+        #rospy.sleep(1)
 
     def get_now_pose(self):
         # 获取机械臂当前位姿
@@ -87,11 +87,11 @@ class MoveIt_Control:
         # 打印位姿信息       
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
-    
+        
     def create_path_constraints2(self,start_point,end_point,r):#创建路径约束
         #计算起点指向终点的向量
         vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
-        height = np.linalg.norm(vector)+0.006  #计算向量或矩阵的范数  用于度量向量或矩阵的大小或长度
+        height = np.linalg.norm(vector)+0.16  #高度延长16cm
         radius = r
         
         # 位置约束
@@ -101,7 +101,6 @@ class MoveIt_Control:
         position_constraint.target_point_offset = Vector3(0, 0, 0)#目标点的偏移量
         position_constraint.weight = 1.0#质量 OR 加权因子？
 
-
         #构建 shape_msgs/SolidPrimitive 消息
         bounding_volume = SolidPrimitive()
         bounding_volume.type = SolidPrimitive.CYLINDER#圆柱
@@ -109,7 +108,6 @@ class MoveIt_Control:
         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#定义位置（找了个几何中心）
@@ -153,7 +151,7 @@ class MoveIt_Control:
         self.arm.set_start_state(state)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
         
         #尝试规划次数
-        b = 16 #尝试规划10次
+        b = 10 #尝试规划5次  无约束5次
         for i in range(b):
             traj = self.arm.plan()#调用plan进行规划
             error=traj[3]
@@ -168,15 +166,19 @@ class MoveIt_Control:
                 trajectory.append(trajj)  
                 break
             else:
+                rospy.loginfo(error)
                 rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                 self.arm.clear_path_constraints()
                 rrr=rrr+0.2#每次增加10厘米
-                rospy.loginfo("R值：{}次重新规划".format(rrr))
+                rospy.loginfo("R值：{}".format(rrr))
                 path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
                 self.arm.set_path_constraints(path_constraints)#设定约束
-                if(i==(b-1)):
-                    rospy.loginfo("所有移动规划尝试失败 规划器停止---请检查工件")
+                if(i>=(b-5)):
+                    rospy.loginfo("所有移动规划尝试失败 取消路径约束")
                     self.arm.clear_path_constraints()
+                if(i==(b-1)):
+                    rospy.loginfo("所有移动规划尝试失败  程序退出")
+                    rospy.set_param("sign_control",0)
                     sys.exit(0)
         #清除约束
         self.arm.clear_path_constraints()
@@ -270,7 +272,6 @@ class MoveIt_Control:
         rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
-        # “up”为自定义姿态，你可以使用“home”或者其他姿态
         self.arm.set_named_target('home')
         rospy.loginfo("get_home end")
         self.arm.go()
@@ -280,7 +281,6 @@ class MoveIt_Control:
     def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
-        # “up”为自定义姿态，你可以使用“home”或者其他姿态
         state = self.arm.get_current_state()
         state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
         self.arm.set_start_state(state)
@@ -439,13 +439,16 @@ def ROS2PY_msgs(msgs,m_sr):
         py_msgs.point.type.append(msgs.points[i].type)
     py_msgs.fail=m_sr.hf_fail.copy()
     py_msgs.shun_xv=msgs.points[0].sequence.copy()
-    for i in range(len(py_msgs.point.type)):
-        print(py_msgs.point.xyz_list[i])
-        print(py_msgs.point.type[i])
+
+    # for i in range(len(py_msgs.point.type)):
+    #     print(py_msgs.point.xyz_list[i])
+    #     print(py_msgs.point.type[i])
     print(py_msgs.shun_xv)
     print(py_msgs.fial)
 
-
+    # for i in range(len(py_msgs.point.type)):
+    #     moveit_server.move_j(py_msgs.point.xyz_list[i])
+    #     #input("任意键继续")
 
 if __name__ =="__main__":
 
@@ -459,7 +462,8 @@ if __name__ =="__main__":
     # trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning_test(folder_path,1,True)
     #执行动作
 
-    ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
+    #ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
+
 
     moveit_server.arm.execute(trajectory_merge)
     

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -46,7 +46,7 @@ if __name__ == "__main__":
     rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
     rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
     rospy.set_param("pitch_of_Verticalweld",np.pi/5)
-    rospy.set_param("culling_radius",7) #焊缝剔除半径
+    rospy.set_param("culling_radius",0) #焊缝剔除半径
     rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
@@ -82,7 +82,7 @@ if __name__ == "__main__":
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()
-
+            command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
             rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)

catkin_ws/src/lstrobot_planning/src/add_octomap.cpp

@@ -1,79 +0,0 @@
-#include <ros/ros.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <octomap_msgs/Octomap.h>
-#include <octomap_msgs/conversions.h>
-#include <octomap/OcTree.h>
-
-void appendScene(const std::string& octoFile)
-{
-    ros::NodeHandle nh("~");
-
-    // 创建 MoveIt MoveGroup Interface
-    moveit::planning_interface::MoveGroupInterface group("manipulator");
-    std::string frameId = group.getPlanningFrame();
-
-    // 从文件加载 OctoMap
-    octomap::OcTree* octree = new octomap::OcTree(octoFile);
-
-    // 将 Octomap 数据转换为 ROS 消息
-    octomap_msgs::Octomap octomap_msg;
-    if (!octomap_msgs::fullMapToMsg(*octree, octomap_msg))
-    {
-        ROS_ERROR("Failed to convert Octomap to ROS message.");
-        return;
-    }
-
-    // 创建 PlanningSceneInterface 实例
-    moveit::planning_interface::PlanningSceneInterface psi;
-
-    // 创建 PlanningScene 消息
-    moveit_msgs::PlanningScene planning_scene_msg;
-    planning_scene_msg.is_diff = true;
-
-    // 设置 Octomap 的相关信息
-    planning_scene_msg.world.octomap.header.frame_id = "base_link";
-    planning_scene_msg.world.octomap.octomap = octomap_msg;
-
-    // 设置Octomap的原点位置
-    // planning_scene_msg.world.octomap.origin.position.x = 0.0;
-    // planning_scene_msg.world.octomap.origin.position.y = 0.0;
-    // planning_scene_msg.world.octomap.origin.position.z = 0.0;
-
-    // // 设置有效的四元数
-    // planning_scene_msg.world.octomap.origin.orientation.x = 0.0;
-    // planning_scene_msg.world.octomap.origin.orientation.y = 0.0;
-    // planning_scene_msg.world.octomap.origin.orientation.z = 0.0;
-    // planning_scene_msg.world.octomap.origin.orientation.w = 1.0;
-
-    planning_scene_msg.world.octomap.header.stamp = ros::Time::now();
-
-    // 将 PlanningScene 消息应用到 MoveIt 中
-    psi.applyPlanningScene(planning_scene_msg);
-
-    delete octree; // 释放内存
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "append_scene_node");
-    ros::NodeHandle nh;
-
-    // 检查是否传入了OctoMap文件名参数
-    if (argc < 2)
-    {
-        ROS_ERROR("Usage: %s [octomap_file]", argv[0]);
-        return 1;
-    }
-
-    std::string octoFile = argv[1];
-
-    appendScene(octoFile);
-
-    // 确保所有更改都已发送给MoveIt!
-    ros::Duration(1.0).sleep(); // 等待一段时间以确保消息被处理
-
-    ROS_INFO("Planning scene updated with OctoMap.");
-
-    return 0;
-}

catkin_ws/src/lstrobot_planning/src/moveit_cartesian_demo.py

@@ -1,148 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-
-import rospy, sys
-import moveit_commander
-from moveit_commander import MoveGroupCommander
-from geometry_msgs.msg import Pose
-from copy import deepcopy
- 
-class MoveItCartesianDemo:
-    def __init__(self):
- 
-        # 初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
- 
-        # 初始化ROS节点
-        rospy.init_node('moveit_cartesian_demo', anonymous=True)
- 
-        # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
-        cartesian = rospy.get_param('~cartesian', True)
-                      
-        # 初始化需要使用move group控制的机械臂中的arm group
-        arm = MoveGroupCommander('manipulator')
-        
-        # 当运动规划失败后，允许重新规划
-        arm.allow_replanning(True)
-        
-        # 设置目标位置所使用的参考坐标系
-        arm.set_pose_reference_frame('base_link')
-                
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        arm.set_goal_position_tolerance(0.001)
-        arm.set_goal_orientation_tolerance(0.001)
-        
-        # 设置允许的最大速度和加速度
-        arm.set_max_acceleration_scaling_factor(0.5)
-        arm.set_max_velocity_scaling_factor(0.5)
-        
-        # 获取终端link的名称
-        end_effector_link = arm.get_end_effector_link()
- 
-        # 控制机械臂先回到初始化位置
-        arm.set_named_target('home')
-        arm.go()
-        rospy.sleep(1)
-                                               
-        # 获取当前位姿数据最为机械臂运动的起始位姿
-        start_pose = arm.get_current_pose(end_effector_link).pose
-                
-        # 初始化路点列表
-        waypoints = []
- 
-        # 如果为True,将初始位姿加入路点列表
-        if cartesian:
-            waypoints.append(start_pose)
-            
-        # 设置路点数据，并加入路点列表，所有的点都加入
-        wpose = deepcopy(start_pose)#拷贝对象
-        wpose.position.z -= 0.2
- 
-        if cartesian:  #如果设置为True，那么走直线
-            waypoints.append(deepcopy(wpose))
-        else:          #否则就走曲线
-            arm.set_pose_target(wpose)  #自由曲线
-            arm.go()
-            rospy.sleep(1)
- 
-        wpose.position.x += 0.15
- 
-        if cartesian:
-            waypoints.append(deepcopy(wpose))
-        else:
-            arm.set_pose_target(wpose)
-            arm.go()
-            rospy.sleep(1)
-        
-        wpose.position.y += 0.1
- 
-        if cartesian:
-            waypoints.append(deepcopy(wpose))
-        else:
-            arm.set_pose_target(wpose)
-            arm.go()
-            rospy.sleep(1)
- 
-        wpose.position.x -= 0.15
-        wpose.position.y -= 0.1
- 
-        if cartesian:
-            waypoints.append(deepcopy(wpose))
-        else:
-            arm.set_pose_target(wpose)
-            arm.go()
-            rospy.sleep(1)
- 
- 
-        #规划过程
- 
-        if cartesian:
-		fraction = 0.0   #路径规划覆盖率
-		maxtries = 100   #最大尝试规划次数
-		attempts = 0     #已经尝试规划次数
-		
-		# 设置机器臂当前的状态作为运动初始状态
-		arm.set_start_state_to_current_state()
-	 
-		# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
-		while fraction < 1.0 and attempts < maxtries:
-        #规划路径 ，fraction返回1代表规划成功
-		    (plan, fraction) = arm.compute_cartesian_path (
-		                            waypoints,   # waypoint poses，路点列表，这里是5个点
-		                            0.01,        # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达
-		                            0.0,         # jump_threshold，跳跃阈值，设置为0代表不允许跳跃
-		                            True)        # avoid_collisions，避障规划
-		    
-		    # 尝试次数累加
-		    attempts += 1
-		    
-		    # 打印运动规划进程
-		    if attempts % 10 == 0:
-		        rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
-		             
-		# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
-		if fraction == 1.0:
-		    rospy.loginfo("Path computed successfully. Moving the arm.")
-		    arm.execute(plan)
-		    rospy.loginfo("Path execution complete.")
-		# 如果路径规划失败，则打印失败信息
-		else:
-		    rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  
- 
-		rospy.sleep(1)
- 
-        # 控制机械臂先回到初始化位置
-        arm.set_named_target('home')
-        arm.go()
-        rospy.sleep(1)
-        
-        # 关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
- 
-if __name__ == "__main__":
-    try:
-        MoveItCartesianDemo()
-    except rospy.ROSInterruptException:
-        pass
-

catkin_ws/src/lstrobot_planning/src/pcd2octomap.cpp

@@ -1,44 +0,0 @@
-#include <iostream>
-#include <assert.h>
-
-//pcl
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-
-//octomap
-#include <octomap/octomap.h>
-using namespace std;
-
-int main( int argc, char** argv )
-{
-    if (argc != 3)
-    {
-        cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl;
-        return -1;
-    }
-
-    string input_file = argv[1], output_file = argv[2];
-    pcl::PointCloud<pcl::PointXYZ> cloud;
-    pcl::io::loadPCDFile<pcl::PointXYZ> ( input_file, cloud );
-
-    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;
-
-    //声明octomap变量
-    cout<<"copy data into octomap..."<<endl;
-    // 创建八叉树对象，参数为分辨率，这里设成了0.05
-    octomap::OcTree tree( 0.005 );
-
-    for (auto p:cloud.points)
-    {
-        // 将点云里的点插入到octomap中
-        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
-    }
-
-    // 更新octomap
-    tree.updateInnerOccupancy();
-    // 存储octomap
-    tree.writeBinary( output_file );
-    cout<<"done."<<endl;
-
-    return 0;
-}

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -426,5 +426,17 @@
   <link name="link_end">
   </link>
 
+  <joint
+  name="joint_end2"
+  type="fixed">
+    <parent
+	link="link_end"/>
+    <child
+	link="link_end_now"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0  0.0  0.01"/>
+  </joint>
+
+  <link name="link_end_now">
+  </link>
 
 </robot>

catkin_ws/src/visual/.vscode/c_cpp_properties.json

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

catkin_ws/src/visual/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "C_Cpp.errorSquiggles": "disabled"
+}

catkin_ws/src/visual/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(visual)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  lstrobot_moveit_config_0605
+)
+
+catkin_package(
+
+)
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    interactive_markers
+    moveit_core
+    moveit_visual_tools
+    moveit_ros_planning
+    moveit_ros_planning_interface
+    moveit_ros_perception
+    pluginlib
+    geometric_shapes
+    pcl_ros
+    pcl_conversions
+    rosbag
+    tf2_ros
+    tf2_eigen
+    tf2_geometry_msgs
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED system filesystem date_time thread)
+
+include_directories(${THIS_PACKAGE_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS})
+
+add_executable(visual_node src/visual_tools.cpp)
+target_link_libraries(visual_node ${catkin_LIBRARIES} )#${Boost_LIBRARIES})
+
+install(TARGETS visual_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

catkin_ws/src/visual/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>visual</name>
+  <version>0.0.0</version>
+  <description>The visual package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robot@todo.todo">robot</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/visual</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

catkin_ws/src/visual/src/visual_tools.cpp

@@ -0,0 +1,210 @@
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <vector>
+
+using namespace std; 
+#define pi 3.1415926
+
+void euler2quat(float phi, float theta, float psi, float quat[4]) {
+    float c1 = cos(phi / 2);
+    float c2 = cos(theta / 2);
+    float c3 = cos(psi / 2);
+    float s1 = sin(phi / 2);
+    float s2 = sin(theta / 2);
+    float s3 = sin(psi / 2);
+    quat[0] = c1*c2*c3 + s1*s2*s3;
+    quat[1] = s1*c2*c3 - c1*s2*s3;
+    quat[2] = c1*s2*c3 + s1*c2*s3;
+    quat[3] = c1*c2*s3 - s1*s2*c3;
+}
+
+ 
+int read_points() {
+    std::ifstream file("points.txt"); // 打开文件
+    if (!file.is_open()) {
+        std::cerr << "无法打开文件" << std::endl;
+        return 1;
+    }
+  
+    std::string line;
+    while (getline(file, line)) { // 逐行读取文件内容
+        std::cout << line << std::endl; // 打印到控制台
+    }
+ 
+    file.close(); // 关闭文件
+    return 0;
+}
+
+//@brief: 指定单个或多个分隔符（单个字符）分割字符串
+//@param: src 原字符串；delimiter 单个或多个分隔符（单个字符）
+vector<string> splitStr(const string& src, const string& delimiter) {
+	std::vector<string> vtStr;
+
+	// 入参检查
+	// 1.原字符串为空返回空 vector
+	if (src == "") {
+		return vtStr;
+	}
+	// 2.分隔符为空返回单个元素为原字符串的 vector
+	if (delimiter == "") {
+		vtStr.push_back(src);
+		return vtStr;
+	}
+
+	string::size_type startPos = 0;
+	auto index = src.find_first_of(delimiter);
+	while (index != string::npos) {
+		auto str = src.substr(startPos, index - startPos);
+		if (str != "") {
+			vtStr.push_back(str);
+		}
+		startPos = index + 1;
+		index = src.find_first_of(delimiter, startPos);
+	}
+	// 取最后一个子串
+	auto str = src.substr(startPos);
+	if (str != "") {
+		vtStr.push_back(str);
+	}
+
+	return vtStr;
+}
+
+int main(int argc, char** argv)
+{
+  float res[4]={0};
+  ros::init(argc, argv, "visual_node");//初始化节点
+  //ros::NodeHandle node_handle;//“大管家”句柄
+  ros::AsyncSpinner spinner(1);//ROS多线程订阅消息，这里开了一个线程？？？
+  spinner.start();//启动线程
+
+  namespace rvt = rviz_visual_tools;//导入rviz_visual_tools这个namespace，同时使用别名rvt
+  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");//创建MoveItVisualTools对象
+
+  visual_tools.deleteAllMarkers();//清除旧的标记（markers）
+  visual_tools.loadRemoteControl();//加载Remote control 主要功能是激活RVIZ里面的“按钮GUI”，操作人员可以通过GUI按钮/键盘来控制程序运行下一步
+  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();//将text_pose初始化为单位阵，再做其他操作。虽然称为3D，实质上为4*4矩阵。
+  text_pose.translation().z() = 1.75;//设置坐标
+  
+  geometry_msgs::Pose target_pose1;///创建一个四元数的位姿
+  geometry_msgs::Pose target_pose2;///创建一个四元数的位姿
+  std::vector<geometry_msgs::Pose> waypoints[20];//定义焊接的路径点列表（创建一个 vector 容器）
+ 
+  //euler2quat(pi,180*pi/180.0,0,res);//ryp角转四元数 设定好姿态
+  // target_pose1.orientation.x=res[0];
+  // target_pose1.orientation.y=res[1];
+  // target_pose1.orientation.z=res[2];
+  // target_pose1.orientation.w = res[3];
+
+  std::string f_path;
+  std::string line;
+  std::vector<string> l;
+
+  ros::param::get("folder_path", f_path);
+  f_path=f_path+"/result.txt";
+  std::ifstream file(f_path); // 打开文件
+  int num=0;
+  if (file.is_open()) 
+  {
+    while (getline(file, line)) 
+    {       
+      l=splitStr(line,"sp,/");
+      target_pose1.position.x = std::stod(l[0])/1000;
+      target_pose1.position.y = std::stod(l[1])/1000;
+      target_pose1.position.z = std::stod(l[2])/1000;
+
+      target_pose2.position.x = std::stod(l[3])/1000;
+      target_pose2.position.y = std::stod(l[4])/1000;
+      target_pose2.position.z = std::stod(l[5])/1000;
+
+      target_pose1.orientation.x= std::stod(l[6]);
+      target_pose1.orientation.y= std::stod(l[7]);
+      target_pose1.orientation.z= std::stod(l[8]);
+      target_pose1.orientation.w = std::stod(l[9]);
+
+      target_pose2.orientation.x= std::stod(l[10]);
+      target_pose2.orientation.y= std::stod(l[11]);
+      target_pose2.orientation.z= std::stod(l[12]);
+      target_pose2.orientation.w = std::stod(l[13]);
+
+      waypoints[num].push_back(target_pose1);//添加到路径点列表
+      waypoints[num].push_back(target_pose2);//添加到路径点列表
+      num++;
+
+      // for (int i=0 ;i<l.size();i++) 
+      // {
+        
+		  //   std::cout << l[i]<< " ";
+	    // }
+      // std::cout<<std::endl;
+    }
+
+  }
+  file.close(); // 关闭文件
+  
+
+  visual_tools.deleteAllMarkers();//清除所有标记
+  visual_tools.publishText(text_pose, "robot", rvt::WHITE, rvt::XLARGE);//文本标记
+  for(int k=0;k<num;k++)
+  {
+  visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
+  for (std::size_t i = 0; i < waypoints[k].size(); ++i)
+    visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (i)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
+  visual_tools.trigger();
+  }
+  // while (ros::ok())
+  // {
+    
+  // }
+  
+  
+  ros::shutdown();
+  return 0;
+}
+/*
+BLACK 	
+BROWN 	
+BLUE 	
+CYAN 	
+GREY 	
+DARK_GREY 	
+GREEN 	
+LIME_GREEN 	
+MAGENTA 	
+ORANGE 	
+PURPLE 	
+RED 	
+PINK 	
+WHITE 	
+YELLOW 	
+TRANSLUCENT 	
+TRANSLUCENT_LIGHT 	
+TRANSLUCENT_DARK 	
+RAND 	
+CLEAR 	
+DEFAULT 
+
+XXXXSMALL 	
+XXXSMALL 	
+XXSMALL 	
+XSMALL 	
+SMALL 	
+MEDIUM 	
+LARGE 	
+XLARGE 	
+XXLARGE 	
+XXXLARGE 	
+XXXXLARGE 	
+REGULAR 
+
+*/

catkin_ws/更新记录.txt

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