#! /usr/bin/env python3
import sys
import rospy
import os
import command
import subprocess
import hjsx
import threading
import numpy as np
import multiprocessing
import actionlib_msgs.msg._GoalStatusArray
from moveit_msgs.msg import PlanningScene
from dynamic_reconfigure.client import Client
from rospy.exceptions import ROSException
from std_srvs.srv import Empty
from rospy.service import ServiceException
from redis_publisher import Publisher


def wait_for_topic(topic_name, message_type):
    try:
        message = rospy.wait_for_message(topic_name, message_type, timeout=None)
        return message
    except rospy.ROSException as e:
        rospy.logerr(f"等待话题 {topic_name} 时程序被中断: {e}")
        return None
    
def clear_octomap():
    try:
        clear_octomap_service = rospy.ServiceProxy('/clear_octomap', Empty)  # 创建服务代理
        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.")

waited_once = False
tim_list = []

if __name__ == "__main__":
    command.load_rviz()
    rospy.init_node("start_node")

    # 初始化各种类型参数
    rospy.set_param("ping_sta", 45)#用于判断平缝的角度
    # rospy.set_param("sign_control",1)
    rospy.set_param("sign_pointcloud", 0)
    rospy.set_param("folder_path", "/home/robot/ROS/ren_yuan/data/123")


    rospy.loginfo("等待rviz启动")
    wait_for_topic("/execute_trajectory/status", actionlib_msgs.msg.GoalStatusArray)
    
    publisher = Publisher()
    publisher.rds.set('sign_control', 0)
    
    while not rospy.is_shutdown():
        # sign_control = str(rospy.get_param("sign_control"))
        sign_control = publisher.rds.get('sign_control').decode('ascii')
        
        if sign_control == "0":
            if not waited_once:
                print("等待点云数据准备完成...")
                waited_once = True
                
        elif sign_control == "1":
            publisher.rds.set('sign_control', 0)
            
            # 清除场景   点云计算并发布
            clear_octomap()
            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
            process.start()

            #计算焊接顺序和焊接姿态
            hjsx.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)

            command.load_visual()

            #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":
            publisher.rds.set('sign_control', 0)
            
            hjsx.run()
            command.load_visual()

            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 == "3":
            rospy.loginfo("正在关闭规划器....")
            command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
            sys.exit(0)

    
    
