#! /usr/bin/env python3
import subprocess
import rospy
import os
import threading

file_path = "moveitServer2"
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  # 进程可能已经不存在
            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"
    subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
def load_rviz():
    command = "roslaunch lstrobot_moveit_config_0605 demo.launch"
    subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
def launch_publish_pointcloud():
    file_name = "dycl_0506.py"
    absolute_path = os.path.join(script_directory, file_name)
    command = "python3 {}".format(absolute_path)
    subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
def launch_moveit_control_server():
    file_name = "moveitServer2.py"
    absolute_path = os.path.join(script_directory, file_name)
    command = "python3 {}".format(absolute_path)
    subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])

def launch_publish_pointcloud_background():
    file_name = "dycl_0506.py"
    absolute_path = os.path.join(script_directory, file_name)
    command = ["python3", absolute_path]
    subprocess.call(command)
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)
def launch_rviz_background():
    process = subprocess.Popen(
                ['roslaunch', 'lstrobot_moveit_config_0605', 'demo.launch'],
                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL
            )
    return process