|
|
@@ -24,6 +24,7 @@ import math
|
|
|
import traceback
|
|
|
from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
|
|
|
import json
|
|
|
+import termios
|
|
|
|
|
|
pi = np.pi
|
|
|
|
|
|
@@ -132,8 +133,8 @@ class MoveIt_Control:
|
|
|
def move_p_flexible(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)
|
|
|
- rrr=0.05#初始允许半径
|
|
|
-
|
|
|
+ rrr=0.09#初始允许半径 9cm
|
|
|
+ er=0
|
|
|
if trajectory:
|
|
|
#起点位置设置为规划组最后一个点
|
|
|
state = self.arm.get_current_state()
|
|
|
@@ -169,7 +170,7 @@ class MoveIt_Control:
|
|
|
rospy.loginfo(error)
|
|
|
rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
|
|
|
self.arm.clear_path_constraints()
|
|
|
- rrr=rrr+0.2#每次增加10厘米
|
|
|
+ rrr=rrr+0.2#每次增加20厘米半径
|
|
|
rospy.loginfo("R值：{}".format(rrr))
|
|
|
if trajectory:
|
|
|
path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
@@ -180,13 +181,13 @@ class MoveIt_Control:
|
|
|
rospy.loginfo("所有移动规划尝试失败 取消路径约束")
|
|
|
self.arm.clear_path_constraints()
|
|
|
if(i==(b-1)):
|
|
|
- rospy.loginfo("所有移动规划尝试失败 程序退出")
|
|
|
- rospy.set_param("sign_control",0)
|
|
|
- sys.exit(0)
|
|
|
+ rospy.loginfo("所有移动规划尝试失败 焊缝起点不可达")
|
|
|
+ er=1
|
|
|
+ break
|
|
|
#清除约束
|
|
|
self.arm.clear_path_constraints()
|
|
|
|
|
|
- return points,trajectory,trajectory_with_type
|
|
|
+ return points,trajectory,trajectory_with_type,er
|
|
|
|
|
|
|
|
|
def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划 只规划当前的点到上一个点
|
|
|
@@ -298,7 +299,7 @@ class MoveIt_Control:
|
|
|
def path_planning(self,folder_path,gohome=True):
|
|
|
file_path_result = os.path.join(folder_path, 'result.txt')
|
|
|
all_data = process_welding_data(file_path_result)
|
|
|
-
|
|
|
+ err=0
|
|
|
points,trajectory,trajectory_with_type = [],[],[]
|
|
|
for i in range(len(all_data)):
|
|
|
rospy.loginfo("共读取到%d条焊缝，开始规划第%d条",len(all_data),i+1)
|
|
|
@@ -308,12 +309,18 @@ class MoveIt_Control:
|
|
|
q2 = all_data[i][3]
|
|
|
|
|
|
point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
|
|
|
- points,trajectory,trajectory_with_type = self.move_p_flexible(point11,points,trajectory,trajectory_with_type)
|
|
|
+ points,trajectory,trajectory_with_type,err = self.move_p_flexible(point11,points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
+ if err==1:#焊缝起点不可达 则跳过此次焊缝和移动的规划 并将焊缝添加到失败列表
|
|
|
+ self.hf_fail.append(welding_sequence[self.hf_num])
|
|
|
+ self.hf_num=self.hf_num+1#焊缝计数
|
|
|
+ continue
|
|
|
+
|
|
|
point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
|
|
|
points,trajectory,trajectory_with_type = self.move_pl(point22,points,trajectory,trajectory_with_type)
|
|
|
+
|
|
|
rospy.loginfo("第%d条焊缝规划完毕",i+1)
|
|
|
if gohome:
|
|
|
- points,trajectory,trajectory_with_type = self.move_p_flexible(self.home_pose,points,trajectory,trajectory_with_type)
|
|
|
+ points,trajectory,trajectory_with_type,err= self.move_p_flexible(self.home_pose,points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
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)
|
|
|
@@ -458,19 +465,52 @@ def ROS2PY_msgs(msgs, m_sr):
|
|
|
# moveit_server.move_j(py_msgs.point.xyz_list[i])
|
|
|
# #input("任意键继续")
|
|
|
|
|
|
+def get_valid_input(factor, default):
|
|
|
+ while True:
|
|
|
+ user_input=input(factor)
|
|
|
+ if user_input=="":
|
|
|
+ return default
|
|
|
+ try:
|
|
|
+ value=float(user_input)
|
|
|
+ if 0<=value<=1:
|
|
|
+ return value
|
|
|
+ else:
|
|
|
+ rospy.logerr("缩放因子必须在[0, 1]范围内，请重新输入!")
|
|
|
+ except ValueError:
|
|
|
+ rospy.logerr("输入值无效，请根据提示重新输入！")
|
|
|
+
|
|
|
+def get_user_input():
|
|
|
+ """获取用户输入的速度和加速度缩放因子，并在控制台中打印信息和错误提示"""
|
|
|
+ DEFUALT_V = 1.0
|
|
|
+ DEFUALT_A = 1.0
|
|
|
+
|
|
|
+ try:
|
|
|
+ termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空输入缓存
|
|
|
+ rospy.loginfo("输入缓存区已清空！")
|
|
|
+
|
|
|
+ vv=get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
|
|
|
+ #a=get_valid_input("请输入加速度缩放因子(0-1, 默认为1): ", DEFUALT_A)
|
|
|
+
|
|
|
+ #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
|
|
|
+ return vv
|
|
|
+ except Exception as e:
|
|
|
+ rospy.logerr(f"发生错误：{e}")
|
|
|
+
|
|
|
+
|
|
|
if __name__ =="__main__":
|
|
|
# from redis_publisher import Publisher
|
|
|
# publisher = Publisher()
|
|
|
|
|
|
folder_path = rospy.get_param("folder_path")
|
|
|
rospy.init_node('moveit_control_server', anonymous=False)
|
|
|
- moveit_server = MoveIt_Control(is_use_gripper=False)
|
|
|
-
|
|
|
+ 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}")
|
|
|
+
|
|
|
trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
|
|
|
-
|
|
|
- # plan_result = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
|
|
|
- # publisher.pub_plan_result(plan_result)
|
|
|
|
|
|
moveit_server.arm.execute(trajectory_merge)
|
|
|
|
bzx