lst_planning

catkin_ws/.catkin_workspace

@@ -1 +0,0 @@
-# This file currently only serves to mark the location of a catkin workspace for tool integration

catkin_ws/src/.vscode/c_cpp_properties.json

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

catkin_ws/src/.vscode/settings.json

@@ -1,12 +0,0 @@
-{
-    "python.autoComplete.extraPaths": [
-        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
-        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
-        "/opt/ros/noetic/lib/python3/dist-packages"
-    ],
-    "python.analysis.extraPaths": [
-        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
-        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
-        "/opt/ros/noetic/lib/python3/dist-packages"
-    ]
-}

catkin_ws/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

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

@@ -1,22 +0,0 @@
-{
-  "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

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

catkin_ws/更新记录.txt

@@ -1,6 +0,0 @@
-2024-9-3
-
-增加 自定义设置速度因子v和加速度因子a接口
-
-2024-9-18
-修改 使用check来检测第六轴翻转，不再限制第六轴关节角度

lstplanning_dev/README.md

@@ -0,0 +1,70 @@
+## 1.运行代码
+```shell
+source /your_path/devel/setup.bash
+roslaunch lstrobot_planning set_update_paramter_p.py
+```  
+
+## 2.package(SRC)
+1. **lstrobot_planning**  
+1.1 **launch**-launch启动文件  
+1.2 **msg**-自定义消息  
+1.3 **scripts**-Python代码
+
+2. **mr12urdf20240605**  
+2.1 **config**-机械臂先关配置文件  
+2.2 **launch**-launch启动文件  
+2.3 **meshes**-机械臂STL文件  
+2.4 **urdf**-机械臂urdf模型  
+
+3. **lstrobot_moveit_config_0605**  
+3.1 **config**-moveit配置文件，常用srdf(修改机器人配置)  
+3.2 **launch**-launch启动文件，包括demo.launch文件 
+
+4. **Data**  
+4.1 点云数据，起始点终点数据，导出的其他的数据(轨迹关节值、末端执行器笛卡尔坐标) 
+
+5. **visual**  
+5.1 规划结果可视化  
+5.2 该功能包需要安装moveit_visual_tools依赖包  
+5.3 可能需要手动订阅/rviz_visual_tools主题  
+
+## 2024.07.31
+- 做了关节限位，规划时不会出现大角度差姿态来回切换
+- 对move_p做了轨迹间路径约束，移动时末端执行器不会走冗余路径
+
+## 2024.08.05
+- 修改rvizload，通过py程序调起rviz
+- 增加rviz启动等待功能
+- 增加close_rviz函数，通过py接口关闭程序时顺带关闭rviz
+- 增加ROS转py成员变量接口  包括：
+	{
+		焊接轨迹点坐标
+		焊接轨迹点隶属类型
+		焊接序号列表
+		焊接轨迹规划失败序号列表
+	}
+- 增加 不重新加载点云和焊接顺序姿态只重新规划机器人路径和运动学逆解的接口
+- 因为是多文件启动 修改程序逻辑防止规划器被2次启动
+
+## 2024.08.07
+- 取消了焊缝剔除 通过延长焊丝坐标系的方式防碰撞（同时修改urdf和config 的end_effector_link）
+- 增加了轨迹可视化节点(visual 注意：该功能需要安装moveit_visual_tools依赖包)
+
+## 2024.09.01
+- 修复从起点规划到第一个失败后的重新设定约束逻辑问题
+
+## 2024.09.02
+- 通过限制6轴自由度解决六轴翻转问题
+
+## 2024.09.03
+- 增加 自定义设置路径速度因子v和加速度因子a的接口
+
+## 2024.09.10
+- 优化 增加焊缝规划失败处理逻辑，如果焊缝起点不可达，则直接跳过本条焊缝，并进行失败标记
+
+## 2024.09.18
+- 优化 使用check来检查第六轴翻转，不再限制第六轴关节角度
+
+## 2024.11.06
+- 优化 修改demo.lanuch的启动方式为后台静默启动
+

lstplanning_dev/src/lstrobot_planning/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/tong/moveir_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/tong/moveir_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

catkin_ws/src/lstrobot_planning/scripts/command.py → lstplanning_dev/src/lstrobot_planning/scripts/command.py

@@ -2,24 +2,30 @@
 import subprocess
 import rospy
 import os
+import threading
 
 file_path = "moveitServer2"
-script_directory = os.path.dirname(os.path.abspath(__file__))
+script_directory = os.path.dirname(os.path.abspath(__file__))   #当前脚本所在的目录
 
 import signal
 
  
 def close_rviz(terminal_name):
+    try:
     # 使用pgrep查找终端进程
-    pgrep_cmd = "pgrep -f '{}'".format(terminal_name)
-    pids = subprocess.check_output(pgrep_cmd, shell=True).strip().split()
-    
-    # 如果找到了匹配的进程ID，发送SIGTERM信号
-    for pid in pids:
-        try:
-            os.kill(int(pid), signal.SIGTERM)
-        except ProcessLookupError:
-            pass  # 进程可能已经不存在
+        pgrep_cmd = "pgrep -f '{}'".format(terminal_name)
+        pids = subprocess.check_output(pgrep_cmd, shell=True).strip().split()
+        
+        # 如果找到了匹配的进程ID，发送SIGTERM信号
+        for pid in pids:
+            try:
+                os.kill(int(pid), signal.SIGTERM)
+            except ProcessLookupError:
+                pass  # 进程可能已经不存在
+            except Exception as e:
+                print("Error killing process{pid}:{e}")
+    except subprocess.CalledProcessError:
+        print(f"No processes found for terminal name '{terminal_name}'")
 
 def load_visual():
     command = "rosrun visual visual_node"
@@ -47,4 +53,10 @@ def launch_moveit_control_server_background():
     file_name = "moveitServer2.py"
     absolute_path = os.path.join(script_directory, file_name)
     command = ["python3", absolute_path]
-    subprocess.call(command)
+    subprocess.call(command)
+def launch_rviz_background():
+    process = subprocess.Popen(
+                ['roslaunch', 'lstrobot_moveit_config_0605', 'demo.launch'],
+                stdout=subprocess.PIPE, stderr=subprocess.PIPE
+            )
+    return process

catkin_ws/src/lstrobot_planning/scripts/decorator_time.py → lstplanning_dev/src/lstrobot_planning/scripts/decorator_time.py

@@ -4,6 +4,7 @@ import time
 
 #定义一个装饰器，用来计算函数运行时间
 #使用方法：在需要计算运行时间的函数前面加上@decorator_time
+#回调函数
 
 def decorator_time(func):
     def wrapper(*args, **kwargs):

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py → lstplanning_dev/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -149,7 +149,7 @@ def compute_pose_R(fangxiang, hanfeng,start_point, end_point):
     rotx = -np.arctan2(y, z)
     roty = np.arctan2(x, np.sqrt(y**2 + z**2))
 
-    # 构造旋转矩阵
+    # 构造旋转矩阵，A为绕x轴的旋转矩阵，B为绕Y轴的旋转矩阵
     A = np.array([[1, 0, 0],
                 [0, np.cos(rotx), -np.sin(rotx)],
                 [0, np.sin(rotx), np.cos(rotx)]])

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py → lstplanning_dev/src/lstrobot_planning/scripts/moveitServer2.py

@@ -60,7 +60,7 @@ class MoveIt_Control:
 
         self.arm.set_num_planning_attempts(10)
         # 机械臂初始姿态
-        #self.move_j()
+        #self.move_j()	  #备注，是否可以用move_j代替go_home来节省时间(待测试)
         self.go_home()
         self.home_pose=self.get_now_pose()
 
@@ -197,7 +197,7 @@ class MoveIt_Control:
         self.arm.clear_path_constraints()
         
         return points,trajectory,trajectory_with_type,er
-    
+
     #TODO move_pl
     def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划  只规划当前的点到上一个点
         
@@ -213,7 +213,7 @@ class MoveIt_Control:
                 
         self.arm.set_pose_target(point, self.end_effector_link)#设定目标点为传入的点
                 
-        # 创建路径约束
+        #创建路径约束
         path_constraints = self.create_path_constraints(points[-1],point)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
         self.arm.set_path_constraints(path_constraints)#设定约束
 
@@ -545,6 +545,7 @@ def ROS2PY_msgs(msgs, m_sr):
     #     moveit_server.move_j(py_msgs.point.xyz_list[i])
     #     #input("任意键继续")
 
+#TODO get_valid_input
 def get_valid_input(factor, default):
     while True:
         user_input=input(factor)
@@ -585,30 +586,6 @@ def ROS2_msgs(msgs, m_sr):
         for point in py_msgs.point.xyz_list:
             file.write(' '.join(str(value) for value in point)+ "\n")
             
-# def write_ee(msgs, m_sr):
-#     for i in range(len(msgs.points)):
-#         py_msgs.point.xyz_list.append(msgs.points[i].positions)
-        
-#     f_p1 = os.path.join(folder_path, 'ee_pos.txt')
-#     with open(f_p1,'w') as file:
-#         for point in py_msgs.point.xyz_list:
-#             #更新机械臂状态
-#             state=m_sr.arm.get_current_state()
-#             #state.joint_state.position=point
-            
-#             #获取末端执行器的位姿
-#             fk_result=m_sr.arm.get_current_pose(moveit_server.end_effector_link)
-            
-#             #获取位置信息
-#             position=fk_result.pose.position
-#             orientation=fk_result.pose.orientation
-            
-#             #格式化数据
-#             pose_data = f"Position: x={position.x}, y={position.y}, z={position.z}\n"
-#             pose_data += f"Orientation (Quaternion): x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}\n"
-#             pose_data += "-" * 50 + "\n"
-            
-#             file.write(pose_data)
 
 if __name__ =="__main__":
     # from redis_publisher import Publisher
@@ -619,14 +596,14 @@ if __name__ =="__main__":
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-
-    speed_v=get_user_input()
-    rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
+    speed_v=1.0
+    # speed_v=get_user_input()
+    # 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)
+    #ROS2_msgs(trajectory_with_type_merge,moveit_server)
     #write_ee(trajectory_with_type_merge, moveit_server)
     
     moveit_server.arm.execute(trajectory_merge)
     
-    rospy.set_param("sign_control",0)
+    rospy.set_param("sign_control",0)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py → lstplanning_dev/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -18,6 +18,8 @@ import actionlib_msgs.msg._GoalStatusArray
 import sys
 import time
 import termios
+import rosnode
+
 def wait_for_topic(topic_name, message_type):
     try:
         message = rospy.wait_for_message(topic_name, message_type, timeout=None)
@@ -38,7 +40,8 @@ waited_once = False
 tim_list=[]
 
 if __name__ == "__main__":
-    command.load_rviz()
+    #command.load_rviz()
+    command.launch_rviz_background()
     rospy.init_node("set_update_paramter_p")
 
     # 初始化各种类型参数
@@ -46,7 +49,7 @@ if __name__ == "__main__":
     rospy.set_param("cishu",0)
     rospy.set_param("time",0)
 
-    rospy.set_param("sign_control",1)
+    rospy.set_param("sign_control",0)
     rospy.set_param("sign_pointcloud",0)
     rospy.set_param("sign_traj_accepted",0)
 
@@ -72,17 +75,19 @@ if __name__ == "__main__":
             if not waited_once:
                 print("等待点云数据准备完成...")
                 termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
-                print("---1.重新加载点云数据并规划---\n---2.仅重新规划---\n---按任意键退出程序---")
+                print("---1.加载点云数据并规划---\n---2.仅重新规划路径---\n---按任意键退出程序---")
                 
-                user_input=input("请选择 1-2: ")
-                #aaa='2'
+                user_input=input("请选择1-2(默认enter为2): ")
+                if user_input=="":
+                    user_input='2'
 
                 rospy.set_param("cishu",rospy.get_param("cishu")+1)
                 if(user_input =='1' or user_input=='2'):
                     rospy.set_param("sign_control", user_input)
                 else:
                     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
-                    exit(0)
+                    sys.exit(0)
+                    #rosnode.kill_nodes(["set_update_paramter_p"])
                 waited_once = True
         elif sign_control == "1":
             # 重置参数

lstplanning_dev/src/visual/.vscode/c_cpp_properties.json

@@ -0,0 +1,38 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/tong/moveir_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**",
+        "/home/tong/moveir_ws/src/geometric_shapes/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/chomp/chomp_interface/include/**",
+        "/home/tong/moveir_ws/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/benchmarks/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/move_group/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/occupancy_map_monitor/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/robot_interaction/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/moveit_servo/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_setup_assistant/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**",
+        "/home/tong/moveir_ws/src/moveit_visual_tools/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
+        "/home/tong/moveir_ws/src/rviz_visual_tools/include/**",
+        "/home/tong/moveir_ws/src/srdfdom/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

lstplanning_dev/src/visual/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/tong/moveir_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/tong/moveir_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

catkin_ws/src/visual/CMakeLists.txt → lstplanning_dev/src/visual/CMakeLists.txt

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