2024-8-7

catkin_ws/src/lstrobot_planning/launch/same_terminal_to_run.launch

@@ -0,0 +1,7 @@
+<launch>
+  <include file="$(find lstrobot_moveit_config_0605)/launch/demo.launch">
+  </include>
+  <node pkg="lstrobot_planning" name="set_update_paramter_p" type="set_update_paramter_p.py" required="true" output="screen">
+  </node>
+
+</launch>

catkin_ws/src/lstrobot_planning/launch/set_update_paramter_p.launch

@@ -2,4 +2,4 @@
   <node pkg="lstrobot_planning" name="set_update_paramter_p" type="set_update_paramter_p.py" required="true" output="screen">
   </node>
 
-</launch>
+</launch>

catkin_ws/src/lstrobot_planning/scripts/11.py

@@ -8,7 +8,15 @@ from subprocess import Popen
 if __name__ == '__main__':
     
     rospy.init_node("test")
-    rospy.set_param("sign_control",1)
+    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

@@ -6,6 +6,21 @@ import os
 file_path = "moveitServer2"
 script_directory = os.path.dirname(os.path.abspath(__file__))
 
+import signal
+
+ 
+def close_rviz(terminal_name):
+    # 使用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  # 进程可能已经不存在
+
 def load_rviz():
     command = "roslaunch lstrobot_moveit_config_0605 demo.launch"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])

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.end_effector_link = self.arm.get_end_effector_link()
         # 设置机械臂基座的参考系
@@ -56,10 +56,22 @@ class MoveIt_Control:
 
         self.arm.set_num_planning_attempts(10)
         # 机械臂初始姿态
-
+        #self.move_j()
         self.go_home()
         self.home_pose=self.get_now_pose()
-        
+
+        # 关节规划，输入6个关节角度（单位：弧度）
+    def move_j(self, joint_configuration=None,a=1,v=1):
+        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
+        if joint_configuration==None:
+            joint_configuration = [0, 0, 0, 0, 0, 0]
+        self.arm.set_max_acceleration_scaling_factor(a)
+        self.arm.set_max_velocity_scaling_factor(v)
+        self.arm.set_joint_value_target(joint_configuration)
+        rospy.loginfo("move_j:"+str(joint_configuration))
+        self.arm.go()
+        rospy.sleep(1)
+
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -457,3 +469,4 @@ if __name__ =="__main__":
     
     #发布规划完毕的轨迹信息
     #pub_trajectories(trajectory_with_type_merge)
+    rospy.set_param("sign_control",0)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -14,11 +14,11 @@ from rospy.exceptions import ROSException
 from std_srvs.srv import Empty
 from rospy.service import ServiceException
 
+import actionlib_msgs.msg._GoalStatusArray
+import sys
 def wait_for_topic(topic_name, message_type):
     try:
-        rospy.loginfo("等待场景地图加载完毕...")
         message = rospy.wait_for_message(topic_name, message_type, timeout=None)
-        rospy.loginfo(f"场景地图已加载完毕")
         return message
     except rospy.ROSException as e:
         rospy.logerr(f"等待话题 {topic_name} 时程序被中断: {e}")
@@ -54,20 +54,29 @@ if __name__ == "__main__":
     rospy.loginfo("yaw = %s",rospy.get_param("yaw")/np.pi*180)
     rospy.loginfo("pitch_of_Horizontalweld = %s",rospy.get_param("pitch_of_Horizontalweld")/np.pi*180)
     rospy.loginfo("pitch_of_Verticalweld = %s",rospy.get_param("pitch_of_Verticalweld")/np.pi*180)
+    rospy.loginfo("等待rviz启动")
+    wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
     
-    while True:
+    while not rospy.is_shutdown():
         sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
             if not waited_once:
                 print("等待点云数据准备完成...")
+                sys.stdin.flush()#清空键盘缓冲区
+                aaa=input("请选择 1-2: ")
+                if(aaa =='1' or aaa=='2'):
+                    rospy.set_param("sign_control",aaa)
+                else:
+                    command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
+                    exit(0)
                 waited_once = True
         elif sign_control == "1":
             # 重置参数
-            rospy.set_param("sign_control",0)
+            #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()
             #计算焊接顺序和焊接姿态
@@ -75,27 +84,33 @@ if __name__ == "__main__":
             hanqiangpose.run()
 
             # 等待 /move_group/monitored_planning_scene 话题发布消息
+            rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")
             
             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
  
         elif sign_control == "2":
             # 重置参数
-            rospy.set_param("sign_control",0)
+            #rospy.set_param("sign_control",0)
             rospy.set_param("sign_traj_accepted",0)
 
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
+            while rospy.get_param("sign_control"):
+                pass
             #运行结束，重置参数
             waited_once = False
-        
 
+    
     

catkin_ws/更新记录.txt

@@ -1,19 +0,0 @@
-2024-7-31
-优化了轨迹规划：
-	做了关节限位 规划时不会出现大角度差姿态来回切换
-	做了轨迹间路径约束 移动时末端执行器不会走冗余路径
-	
-2024-8-5
-	修改rvizload
-	修改点云加载 不再保存又立即读出
-	修改轨迹规划 去除check 转而判断规划成功与否
-	增加ROS转py成员变量接口
-	ROS消息转PY类
-	增加不重新加载点云和焊接顺序姿态等  只重新规划机器人路径和运动学逆解的接口
-	
-	
-	
-	
-	做初始化优化   go home (搞不了 是系统封装的函数   而且好像是因为第一次要加载点云到碰撞检测)
-	因为是多文件启动 要防止规划器被2次启动
-