import math
import os
import subprocess
import tempfile

import rospy
from gazebo_msgs.srv import SpawnModel
from geometry_msgs.msg import Point, Pose, Quaternion
from tf.transformations import quaternion_from_euler


class GazeboWorldManager:
    def __init__(self):
        rospy.init_node('gazebo_world_manager')
        self.spawn_service = rospy.ServiceProxy(
            '/gazebo/spawn_urdf_model', SpawnModel)

    def spawn_apriltag_marker(self, model_name, tag_id, x=0, y=0, z=1):
        """生成 AprilTag 标记模型"""

        xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/fixed_tag.xacro"

        try:
            # 添加包路径到 ROS_PACKAGE_PATH
            ros_package_path = os.environ.get('ROS_PACKAGE_PATH', '')
            custom_path = '/home/leju/gazebo_world_manager'
            if custom_path not in ros_package_path.split(':'):
                os.environ['ROS_PACKAGE_PATH'] = f"{custom_path}:{ros_package_path}" if ros_package_path else custom_path

            # 转换为 URDF
            urdf_content = subprocess.check_output([
                'rosrun', 'xacro', 'xacro', xacro_file, f'tag_id:={tag_id}'
            ]).decode('utf-8')

            # 设置位姿
            pose = Pose(
                position=Point(x=x, y=y, z=z),
                orientation=Quaternion(*quaternion_from_euler(0, 0, 0))
            )

            # 生成模型
            rospy.wait_for_service('/gazebo/spawn_urdf_model')
            response = self.spawn_service(
                model_name=model_name,
                model_xml=urdf_content,
                robot_namespace='',
                initial_pose=pose,
                reference_frame='world'
            )

            if response.success:
                rospy.loginfo(f"Successfully spawned {model_name}")
            else:
                rospy.logerr(
                    f"Failed to spawn {model_name}: {response.status_message}")
        except subprocess.CalledProcessError as e:
            rospy.logerr(f"Failed to convert xacro to urdf: {e}")


if __name__ == '__main__':
    manager = GazeboWorldManager()
    manager.spawn_apriltag_marker('mk', 0, 1, 1, 1.4)
    # rospy.spin()