lst_planning

lstplanning_dev/src/lstrobot_planning/README.md

@@ -0,0 +1,76 @@
+## 1.运行代码
+```shell
+source /your_path/devel/setup.bash
+roslaunch lstrobot_planning set_update_paramter_p.launch
+```  
+
+## 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的启动方式为后台静默启动
+
+## 2024.11.16
+- 修复 修复因使用进程进行后台静默启动导致的缓冲区溢出问题(导致程序卡死)；目前解决办法为将
+  输出直接丢弃或是重定向到log文件
+- 修改 将点云的加载方式改为从ROS参数服务器获取对应控制参数
+
+

lstplanning_dev/src/lstrobot_planning/scripts/command.py

@@ -57,6 +57,6 @@ def launch_moveit_control_server_background():
 def launch_rviz_background():
     process = subprocess.Popen(
                 ['roslaunch', 'lstrobot_moveit_config_0605', 'demo.launch'],
-                stdout=subprocess.PIPE, stderr=subprocess.PIPE
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL
             )
     return process

lstplanning_dev/src/lstrobot_planning/scripts/dycl_0506.py

@@ -137,7 +137,6 @@ def talker():
     # pub2 = rospy.Publisher("/pointcloud/output2", PointCloud2, queue_size=10)
     rospy.init_node('publish_pointcloud', anonymous=True)
     rate = rospy.Rate(10)
-
     
     points1 = np.asarray(ptCloud_scaled1.points)
     points2 = np.asarray(ptCloud_scaled2.points)

lstplanning_dev/src/lstrobot_planning/scripts/moveitServer2.py

@@ -240,7 +240,7 @@ class MoveIt_Control:
         self.hf_num=self.hf_num+1#焊缝计数
         return points,trajectory,trajectory_with_type
 
-    #TODO move_pl_check
+    #TODO move_pl_check暂未使用
     def move_pl_check(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)

lstplanning_dev/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -18,7 +18,6 @@ import actionlib_msgs.msg._GoalStatusArray
 import sys
 import time
 import termios
-import rosnode
 
 def wait_for_topic(topic_name, message_type):
     try:
@@ -31,10 +30,11 @@ def wait_for_topic(topic_name, message_type):
 def clear_octomap():
     try:
         clear_octomap_service = rospy.ServiceProxy('/clear_octomap', Empty)  # 创建服务代理
+        clear_octomap_service.wait_for_service()    #等待服务准备完成
         clear_octomap_service()  # 尝试调用服务
         rospy.loginfo("Octomap has been cleared.")
-    except ServiceException as e:
-        rospy.loginfo("Failed to call /clear_octomap service or it is not available. Skipping operation.")
+    except rospy.ServiceException as e:
+        rospy.logerr(f"Failed to call /clear_octomap service or it is not available: {e}. Skipping operation.")
 
 waited_once = False
 tim_list=[]
@@ -58,8 +58,8 @@ if __name__ == "__main__":
     rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
     rospy.set_param("pitch_of_Verticalweld",np.pi/5)
     rospy.set_param("culling_radius",0) #焊缝剔除半径
-    rospy.set_param("folder_path","/root/catkin_ws/data")     #4条焊缝
-    #rospy.set_param("folder_path","/home/tong/moveir_ws/data/6_Welds")      #6条焊缝
+    #rospy.set_param("folder_path","/home/tong/catkin_ws/data/20240913-1")     #4条焊缝
+    rospy.set_param("folder_path","/home/tong/catkin_ws/data/6_Welds")      #6条焊缝
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
     rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
@@ -74,20 +74,19 @@ if __name__ == "__main__":
         if sign_control == "0":
             if not waited_once:
                 print("等待点云数据准备完成...")
-                termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
-                print("---1.加载点云数据并规划---\n---2.仅重新规划路径---\n---按任意键退出程序---")
+                # termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
+                # print("---1.加载点云数据并规划---\n---2.仅重新规划路径---\n---按任意键退出程序---")
                 
-                user_input=input("请选择1-2(默认enter为2): ")
-                if user_input=="":
-                    user_input='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")
-                    sys.exit(0)
-                    #rosnode.kill_nodes(["set_update_paramter_p"])
+                # 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")
+                #     sys.exit(0)
                 waited_once = True
         elif sign_control == "1":
             # 重置参数
@@ -126,6 +125,7 @@ if __name__ == "__main__":
             rospy.set_param("sign_traj_accepted",0)
             hjsx.run()
             hanqiangpose.run()
+            command.load_visual()
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
@@ -141,5 +141,8 @@ if __name__ == "__main__":
             #运行结束，重置参数
             waited_once = False
 
-    
+        else:
+            rospy.loginfo("正在关闭规划程序...")
+            command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
+            exit(0)