lstrobot_planning

lstplanning_dev/README.md

@@ -73,4 +73,14 @@ roslaunch lstrobot_planning set_update_paramter_p.launch
   输出直接丢弃或是重定向到log文件
 - 修改 将点云的加载方式改为从ROS参数服务器获取对应控制参数
 
-
+## 2024.11.21
+- 修复 第一条焊缝move_p出现TimeOut错误后无法正常跳过本条焊缝，并继续规划下一条焊缝的问题
+- 修改 不再通过ROS参数服务器管理相关参数，全部通过redis进行参数设置 (待完成)
+- 待优化 第一次move_p如果规划超时(Timeout)，则尝试调用compute_cartesian_path接口进行move_p规划
+	    如果成功，则继续进行move_l焊缝规划
+
+## 2024.11.28
+- 精度问题解决，原因为相机手眼标定后未更新变换矩阵
+- 实际运动过程中，末端变换姿态会导致误差增大  原因查找中，只出现在测试电脑中！！！基本确定为moveit版本问题(1.1.16)
+- 增加轨迹平滑函数，通过三次样条插值平滑轨迹
+- 增加检查焊缝数据是否存在的接口，避免data中无数据时程序崩溃问题

lstplanning_dev/src/lstrobot_planning/scripts/moveitServer2.py

@@ -29,6 +29,7 @@ import termios
 from decorator_time import decorator_time
 import check
 from redis_publisher import Publisher
+from scipy.interpolate import CubicSpline
 
 pi = np.pi
 
@@ -94,6 +95,34 @@ class MoveIt_Control:
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
     
+    #TODO 使用三次样条插值平滑轨迹
+    def smooth_trajectory(self, trajectory, num_points=0):
+        
+        # print(f"原始轨迹点数: {len(trajectory.joint_trajectory.points)}")
+        num_points = len(trajectory.joint_trajectory.points) + 50
+        # 创建平滑后的轨迹
+        smoothed_trajectory = []
+        for i in range(len(trajectory.joint_trajectory.joint_names)):
+            joint_trajectory = [point.positions[i] for point in trajectory.joint_trajectory.points]
+            cs = CubicSpline(range(len(joint_trajectory)), joint_trajectory)  #创建三次样条插值器
+            smooth_joint_trajectory = cs(np.linspace(0, len(joint_trajectory) - 1, num_points))  #生成平滑后的轨迹
+            smoothed_trajectory.append(smooth_joint_trajectory)
+
+        # 创建平滑后的轨迹点
+        smoothed_trajectory_points = []
+        for j in range(num_points):
+            point = trajectory.joint_trajectory.points[0]
+            smoothed_point = JointTrajectoryPoint()
+            smoothed_point.positions = [smoothed_trajectory[i][j] for i in range(len(smoothed_trajectory))]
+            smoothed_point.time_from_start = rospy.Duration(j * 0.05)  # 控制每个点之间的时间间隔
+            smoothed_trajectory_points.append(smoothed_point)
+
+        # 将平滑后的轨迹生成新的 JointTrajectory
+        trajectory.joint_trajectory.points = smoothed_trajectory_points
+        # print(f"平滑后的轨迹点数: {len(trajectory.joint_trajectory.points)}")
+        
+        return trajectory
+    
     #TODO move_p 路径约束   
     def create_path_constraints2(self, start_point, end_point, r):
         #计算起点指向终点的向量
@@ -174,7 +203,13 @@ class MoveIt_Control:
                     trajj_with_type = mark_the_traj(trajj,"during-p", welding_sequence)     
                     trajectory_with_type.append(trajj_with_type)
                     points.append(point)
-                    trajectory.append(trajj)  
+                    trajectory.append(trajj)
+                    #平滑轨迹
+                    # smooth_traj = self.smooth_trajectory(trajj)
+                    # trajj_with_type = mark_the_traj(smooth_traj,"during-p", welding_sequence)     
+                    # trajectory_with_type.append(trajj_with_type)
+                    # points.append(point)
+                    # trajectory.append(smooth_traj)    
                     break
                 else:
                     rospy.loginfo("check failed! 移动轨迹无效")
@@ -305,6 +340,14 @@ class MoveIt_Control:
             points.append(point)
             trajectory.append(trajj)
             trajectory_with_type.append(trajj_with_type)
+            #平滑轨迹
+            # smoothed_plan = self.smooth_trajectory(trajj)
+            # trajj_with_type = mark_the_traj(smoothed_plan, "during-l", welding_sequence)
+            # trajj_with_type.points[-len(smoothed_plan.joint_trajectory.points)].type = "start"  
+            # trajj_with_type.points[-1].type = "end"
+            # points.append(point)
+            # trajectory.append(smoothed_plan)
+            # trajectory_with_type.append(trajj_with_type)
         else:
             rospy.loginfo("本次焊缝规划失败")
             points.pop()    #将本条焊缝的起点从规划中删除

lstplanning_dev/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -36,6 +36,16 @@ def clear_octomap():
     except rospy.ServiceException as e:
         rospy.logerr(f"Failed to call /clear_octomap service or it is not available: {e}. Skipping operation.")
 
+def check_file():
+    file_path = rospy.get_param("folder_path")
+    file_path_points = os.path.join(file_path, 'points.txt')
+    if os.path.exists(file_path_points):
+        result = True
+    else:
+        rospy.loginfo("焊缝数据不存在，请选择焊缝...")
+        result = False
+    return result
+
 waited_once = False
 tim_list=[]
 
@@ -93,58 +103,68 @@ if __name__ == "__main__":
                 #     sys.exit(0)
                 waited_once = True
         elif sign_control == "1":
-            # 重置参数
-            #rospy.set_param("sign_control",0)    #在轨迹规划里重置 防止重复调用
-            rospy.set_param("sign_traj_accepted",0)
-            # 清除场景
-            clear_octomap()
-            # 点云计算并发布
-            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            process.start()
-            #计算焊接顺序和焊接姿态
-            hjsx.run()
-            hanqiangpose.run()
-            # command.load_visual()
-            # 等待 /move_group/monitored_planning_scene 话题发布消息
-            rospy.loginfo("等待场景地图加载完毕...")
-            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            rospy.loginfo("场景地图已加载完毕")
-            rospy.set_param("sign_pointcloud",1)
-            #rospy.loginfo("终止点云发布，关闭发布窗口")
-            #input("按任意键继续...")
-            
-            rospy.loginfo("运行moveitserver2.py")
-            process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
-            process.start()
-
-            while rospy.get_param("sign_control"):
-                pass
-            #运行结束，重置参数
-            waited_once = False
+            flag = check_file()
+            if flag:
+                # 重置参数
+                #rospy.set_param("sign_control",0)    #在轨迹规划里重置 防止重复调用
+                rospy.set_param("sign_traj_accepted",0)
+                # 清除场景
+                clear_octomap()
+                # 点云计算并发布
+                process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+                process.start()
+                #计算焊接顺序和焊接姿态
+                hjsx.run()
+                hanqiangpose.run()
+                # command.load_visual()
+                # 等待 /move_group/monitored_planning_scene 话题发布消息
+                rospy.loginfo("等待场景地图加载完毕...")
+                wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+                rospy.loginfo("场景地图已加载完毕")
+                rospy.set_param("sign_pointcloud",1)
+                #rospy.loginfo("终止点云发布，关闭发布窗口")
+                #input("按任意键继续...")
+                
+                rospy.loginfo("运行moveitserver2.py")
+                process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
+                process.start()
+
+                while rospy.get_param("sign_control"):
+                    pass
+                #运行结束，重置参数
+                waited_once = False
+            else:
+                rospy.set_param("sign_control",0)
+                waited_once = False
  
         elif sign_control == "2":
-            tim=time.time()
-            rospy.set_param("time",0)
-            # 重置参数
-            #rospy.set_param("sign_control",0)
-            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()
-
-            while rospy.get_param("sign_control"):
-                pass
-
-            tim=time.time()-tim
-            tim_list.append(tim)
-            print("-------------------------------------")
-            print(f'第{rospy.get_param("cishu")}次规划：')
-            print(tim_list)
-            print("")
-            #运行结束，重置参数
-            waited_once = False
+            flag = check_file()
+            if flag:
+                tim=time.time()
+                rospy.set_param("time",0)
+                # 重置参数
+                #rospy.set_param("sign_control",0)
+                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()
+
+                while rospy.get_param("sign_control"):
+                    pass
+
+                tim=time.time()-tim
+                tim_list.append(tim)
+                print("-------------------------------------")
+                print(f'第{rospy.get_param("cishu")}次规划：')
+                print(tim_list)
+                print("")
+                #运行结束，重置参数
+                waited_once = False
+            else:
+                rospy.set_param("sign_control",0)
+                waited_once = False
 
         else:
             rospy.loginfo("正在关闭规划程序...")