import os
import subprocess

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


class GraspModelGenerator:
    def __init__(self):
        rospy.init_node('gazebo_world_manager', anonymous=True)
        self.spawn_service = rospy.ServiceProxy(
            '/gazebo/spawn_urdf_model', SpawnModel)
        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

    def _generate(self, model_name, urdf_content, pose):
        try:
            # 生成模型
            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}")

    def generate_table(self, name, x, y, height):
        xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/table.xacro"
        urdf_content = subprocess.check_output([
            'rosrun', 'xacro', 'xacro', xacro_file,
            f'x:={x}', f'y:={y}', f'height:={height}'
        ]).decode('utf-8')
        pose = Pose(
            position=Point(x=x, y=y, z=0),
            orientation=Quaternion(*quaternion_from_euler(0, 0, 0))
        )
        self._generate(name, urdf_content, pose)

    def generate_apriltag(self, name, tag_id=0, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
        """生成AprilTag标记模型"""
        xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/fixed_tag.xacro"
        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(roll, pitch, yaw))
        )
        self._generate(name, urdf_content, pose)

    def generate_box_with_apriltag(self, name, l=0.25, w=0.2, h=0.2, mass=1, tag_id=0, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
        """生成带有AprilTag的盒子模型"""
        xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/box.xacro"
        urdf_content = subprocess.check_output([
            'rosrun', 'xacro', 'xacro', xacro_file, f'box_length:={l}', f'box_weight:={w}', f'box_height:={h}', f'box_mass:={mass}', f'tag_id:={tag_id}'
        ]).decode('utf-8')
        pose = Pose(
            position=Point(x=x, y=y, z=z),
            orientation=Quaternion(
                *quaternion_from_euler(roll, pitch, yaw))
        )
        self._generate(name, urdf_content, pose)


if __name__ == '__main__':
    generator = GraspModelGenerator()
    generator.generate_apriltag("tg2", 0, 2, 0, 1.1, 0, 1.57, 1.57)