feat: 实现场景修改，特殊物品添加

gazebo_ctrl.py

@@ -0,0 +1,169 @@
+import rospy
+from gazebo_msgs.srv import (GetLinkProperties, GetModelProperties,
+                             GetModelPropertiesResponse, GetModelState,
+                             GetModelStateResponse, GetWorldProperties)
+from std_srvs.srv import Empty
+
+
+class GazeboROSController:
+    def __init__(self):
+        rospy.init_node('gazebo_ros_controller', anonymous=True)
+        self.model_info = None
+
+    def get_model_names(self) -> list:
+        rospy.wait_for_service('/gazebo/get_world_properties')
+        try:
+            get_world_properties = rospy.ServiceProxy(
+                '/gazebo/get_world_properties', GetWorldProperties)
+            world_properties = get_world_properties()
+            return world_properties.model_names
+        except rospy.ServiceException as e:
+            rospy.logerr(f"Service call failed: {e}")
+            return []
+
+    def get_model_state(self, model_names: list = None) -> dict:
+        """
+        获取模型的状态。
+        如果model_names为空，返回所有模型的状态，否则返回指定模型的状态。
+        """
+        if not model_names or len(model_names) == 0:
+            model_names = self.get_model_names()
+        print(f"Getting state for models: {model_names}")
+        model_states = {}
+        for name in model_names:
+            rospy.wait_for_service('/gazebo/get_model_state')
+            try:
+                get_model_state = rospy.ServiceProxy(
+                    '/gazebo/get_model_state', GetModelState)
+                state = get_model_state(name, '')
+                model_states[name] = state
+            except rospy.ServiceException as e:
+                rospy.logerr(f"Service call failed for model {name}: {e}")
+                model_states[name] = None
+        return model_states
+
+    def get_model_properties(self, model_names: list = None) -> dict:
+        """
+        获取模型的属性。
+        如果model_names为空，返回所有模型的属性，否则返回指定模型的属性。
+        """
+        if not model_names or len(model_names) == 0:
+            model_names = self.get_model_names()
+        print(f"Getting properties for models: {model_names}")
+        model_properties = {}
+        for name in model_names:
+            rospy.wait_for_service('/gazebo/get_model_properties')
+            try:
+                get_model_properties = rospy.ServiceProxy(
+                    '/gazebo/get_model_properties', GetModelProperties)
+                properties = get_model_properties(name)
+                model_properties[name] = properties
+            except rospy.ServiceException as e:
+                rospy.logerr(f"Service call failed for model {name}: {e}")
+                model_properties[name] = None
+        return model_properties
+
+    def get_link_properties(self, link_full_name: str) -> dict:
+        """
+        获取指定链接的属性。
+        """
+        rospy.wait_for_service('/gazebo/get_link_properties')
+        try:
+            get_link_properties = rospy.ServiceProxy(
+                '/gazebo/get_link_properties', GetLinkProperties)
+            properties = get_link_properties(link_full_name)
+            return properties
+        except rospy.ServiceException as e:
+            rospy.logerr(f"Service call failed for link {link_full_name}: {e}")
+            return None
+
+    def format_model_state(self, state: GetModelStateResponse) -> dict:
+        return {
+            'pose': {
+                'position': {
+                    'x': state.pose.position.x,
+                    'y': state.pose.position.y,
+                    'z': state.pose.position.z
+                },
+                'orientation': {
+                    'x': state.pose.orientation.x,
+                    'y': state.pose.orientation.y,
+                    'z': state.pose.orientation.z,
+                    'w': state.pose.orientation.w
+                }
+            },
+            'twist': {
+                'linear': {
+                    'x': state.twist.linear.x,
+                    'y': state.twist.linear.y,
+                    'z': state.twist.linear.z
+                },
+                'angular': {
+                    'x': state.twist.angular.x,
+                    'y': state.twist.angular.y,
+                    'z': state.twist.angular.z
+                }
+            }}
+
+    def format_model_properties(self, properties: GetModelPropertiesResponse) -> dict:
+        return {
+            'parent_model_name': properties.parent_model_name,
+            'canonical_body_name': properties.canonical_body_name,
+            'body_names': properties.body_names,
+            'geom_names': properties.geom_names,
+            'joint_names': properties.joint_names,
+            'child_model_names': properties.child_model_names,
+            'is_static': properties.is_static
+        }
+
+    def format_model_info(self, model_name: str, state: GetModelStateResponse, properties: GetModelPropertiesResponse) -> dict:
+        return {
+            'name': model_name,
+            'state': self.format_model_state(state),
+            'properties': self.format_model_properties(properties)
+        }
+
+    def get_model_info_f(self, model_names: list = None) -> list:
+        """
+        获取模型的详细信息，包括状态和属性。
+        如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。
+        返回值已经格式化为list-dict形式。
+        """
+        if not model_names or len(model_names) == 0:
+            model_names = self.get_model_names()
+        print(f"Getting info for models: {model_names}")
+        model_info = []
+        states = self.get_model_state(model_names)
+        properties = self.get_model_properties(model_names)
+        for name in model_names:
+            m_state = states[name]
+            m_properties = properties[name]
+            info = self.format_model_info(name, m_state, m_properties)
+            model_info.append(info)
+        return model_info
+
+    def get_model_state_f(self, model_names: list = None) -> list:
+        """
+        获取模型的状态。
+        如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。
+        返回值已经格式化为list-dict形式。
+        """
+        if not model_names or len(model_names) == 0:
+            model_names = self.get_model_names()
+        print(f"Getting info for models: {model_names}")
+        model_info = []
+        states = self.get_model_state(model_names)
+        for name in model_names:
+            m_state = states[name]
+            info = self.format_model_state(m_state)
+            info['name'] = name
+            # 获取箱子质量
+            if name == 'block':
+                info['mass'] = self.get_block_mass()
+            model_info.append(info)
+        return model_info
+
+if __name__ == '__main__':
+    controller = GazeboROSController()
+    import json
+    print(json.dumps(controller.get_model_state_f(['block'])))

http_server.py

@@ -0,0 +1,73 @@
+import dataclasses
+import socket
+
+import flask
+import werkzeug.serving
+
+
+@dataclasses.dataclass
+class HttpHost():
+    """
+    HTTP主机信息
+    """
+    hostname: str
+    port: int
+
+    def __post_init__(self):
+        if not self.hostname:
+            raise ValueError("Hostname cannot be empty")
+        if not (0 <= self.port <= 65535):
+            raise ValueError(f"Invalid port: {self.port}")
+
+class HttpRPCServer():
+    """
+    来自ability-sdk-python的http服务器。
+    HTTP RPC服务端实现。
+    使用add_route方法添加路由，重写类以实现自定义功能。
+    """
+
+    def __init__(self):
+        self._server = None
+        self._app = flask.Flask(__name__)
+
+    def add_route(self, uri: str, handler, methods: list):
+        """
+        添加HTTP路由，handler应符合Flask视图函数的规范。
+        """
+        if not callable(handler):
+            raise ValueError("Handler must be a callable function")
+        self._app.add_url_rule(uri, view_func=handler, methods=methods)
+
+    def start(self, hostname: str = "0.0.0.0", port: int = None):
+        """
+        启动并以阻塞方式提供服务。
+        """
+        if self._server:
+            raise RuntimeError("Server is already running")
+        self.host = HttpHost(
+            hostname=hostname, port=port if port else self.find_free_port())
+        self._server = werkzeug.serving.make_server(
+            self.host.hostname, self.host.port, self._app, threaded=True)
+        self._server.serve_forever()
+
+    def stop(self):
+        """
+        停止服务。
+        """
+        if self._server:
+            self._server.shutdown()
+            self._server.server_close()
+            self._server = None
+
+    def find_free_port(self):
+        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+            s.bind(("127.0.0.1", 0))
+            return s.getsockname()[1]
+
+class GazeboSimHttpServer(HttpRPCServer):
+    """
+    Gazebo仿真环境的HTTP服务器。
+    """
+
+    def __init__(self):
+        super().__init__()

main.py

@@ -0,0 +1,63 @@
+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()

package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gazebo_world_manager</name>
+  <version>0.0.0</version>
+  <description>The gazebo_world_manager package</description>
+
+  <maintainer email="user@example.com">user</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rospy</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>geometry_msgs</depend>
+</package>

scene/grasp-box/launch.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+# 启动仿真程序

scene/grasp-box/models/apriltag/materials/apriltag_0.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker0
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-0.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_1.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker1
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-1.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_2.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker2
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-2.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_3.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker3
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-3.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_4.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker4
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-4.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_5.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker5
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-5.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_6.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker6
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-6.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_7.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker7
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-7.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_8.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker8
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-8.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/materials/apriltag_9.material

@@ -0,0 +1,18 @@
+material AprilTag/Marker9
+{
+  technique
+  {
+    pass
+    {
+      ambient 1.0 1.0 1.0 1.0
+      diffuse 1.0 1.0 1.0 1.0
+      specular 0.0 0.0 0.0 1.0
+      texture_unit
+      {
+        texture textures/tag36h11-9.png
+        filtering trilinear
+        scale 1.0 1.0
+      }
+    }
+  }
+}

scene/grasp-box/models/apriltag/urdf/apriltag.xacro

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="apriltag_marker">
+
+  <xacro:macro name="apriltag_marker" params="parent_link *origin id">
+    <!-- 定义二维码的Link -->
+    <link name="apriltag_marker_${id}">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.11 0.11 0.001"/>
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <box size="0.11 0.11 0.001"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="apriltag_joint_${id}" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}"/>
+      <child link="apriltag_marker_${id}"/>
+    </joint>
+
+    <gazebo reference="apriltag_marker_${id}">
+      <visual>
+        <material>
+          <script>
+            <uri>file://$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/materials/apriltag_${id}.material</uri>
+            <name>AprilTag/Marker${id}</name>
+          </script>
+          <shader type="pixel">
+            <normal_map>__default__</normal_map>
+          </shader>
+          <double_sided>false</double_sided>
+        </material>
+      </visual>
+    </gazebo>
+  </xacro:macro>
+</robot>

scene/grasp-box/models/apriltag/urdf/apriltag_cube.xacro

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- 引入apriltag -->
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag.xacro"/>
+
+  <!-- 左臂AprilTag标记 -->
+  <xacro:macro name="left_arm_apriltags" params="parent_link">
+    <xacro:apriltag_marker parent_link="${parent_link}" id="0">
+      <origin xyz="0.055 0 -0.165" rpy="0 1.5708 0"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="1">
+      <origin xyz="-0.055 0 -0.165" rpy="0 1.5708 3.1415"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="2">
+      <origin xyz="0 0.055 -0.165" rpy="0 1.5708 1.5708"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="3">
+      <origin xyz="0 -0.055 -0.165" rpy="0 1.5708 -1.5708"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="4">
+      <origin xyz="0 0 -0.22" rpy="0 3.1416 0"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="111">
+      <origin xyz="0 0 -0.11" rpy="0 3.1416 0"/>
+    </xacro:apriltag_marker>
+  </xacro:macro>
+
+  <!-- 右臂AprilTag标记 -->
+  <xacro:macro name="right_arm_apriltags" params="parent_link">
+    <xacro:apriltag_marker parent_link="${parent_link}" id="5">
+      <origin xyz="0.055 0 -0.165" rpy="0 1.5708 0"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="6">
+      <origin xyz="-0.055 0 -0.165" rpy="0 1.5708 3.1415"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="7">
+      <origin xyz="0 0.055 -0.165" rpy="0 1.5708 1.5708"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="8">
+      <origin xyz="0 -0.055 -0.165" rpy="0 1.5708 -1.5708"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="9">
+      <origin xyz="0 0 -0.22" rpy="0 3.1416 0"/>
+    </xacro:apriltag_marker>
+    <xacro:apriltag_marker parent_link="${parent_link}" id="999">
+      <origin xyz="0 0 -0.11" rpy="0 3.1416 0"/>
+    </xacro:apriltag_marker>
+  </xacro:macro>
+
+</robot>

scene/grasp-box/models/apriltag/urdf/block.urdf.xacro

@@ -0,0 +1,93 @@
+<?xml version="1.0"?>
+<robot name="block">
+  <link name="world"/>
+  
+  <link name="base_link">
+    <!-- 主体视觉和碰撞 -->
+    <visual>
+      <geometry>
+        <box size="0.2 0.2 0.2"/>
+      </geometry>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <material name="red">
+        <color rgba="1 0 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.2 0.2 0.2"/>
+      </geometry>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <surface>
+        <contact>
+          <max_contacts>10</max_contacts>
+          <ode>
+            <kp>1e6</kp>
+            <kd>100</kd>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+
+    <!-- 左耳 -->
+    <visual>
+      <geometry>
+        <box size="0.1 0.15000000000000002 0.025"/>
+      </geometry>
+      <origin xyz="-0.125 0.0 0.07500000000000001" rpy="0 0 0"/>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.15000000000000002 0.025"/>
+      </geometry>
+      <origin xyz="-0.125 0.0  0.07500000000000001" rpy="0 0 0"/>
+      <surface>
+        <contact>
+          <max_contacts>10</max_contacts>
+          <ode>
+            <kp>1e6</kp>
+            <kd>100</kd>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+
+    <!-- 右耳 -->
+    <visual>
+      <geometry>
+        <box size="0.1 0.15000000000000002 0.025"/>
+      </geometry>
+      <origin xyz="0.125 0.0 0.07500000000000001" rpy="0 0 0"/>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.15000000000000002 0.025"/>
+      </geometry>
+      <origin xyz="0.125 0.0 0.07500000000000001" rpy="0 0 0"/>
+      <surface>
+        <contact>
+          <max_contacts>10</max_contacts>
+          <ode>
+            <kp>1e6</kp>
+            <kd>100</kd>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+
+    <!-- 质量惯性（合并计算） -->
+    <inertial>
+      <mass value="1.5"/>
+      <inertia ixx="0.010000000000000002" iyy="0.010000000000000002"  izz="0.010000000000000002" ixy="0" iyz="0" ixz="0"/>
+    </inertial>
+  </link>
+
+  <!-- Floating joint to make the block freely movable -->
+  <joint name="floating" type="floating">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 -1.0 0.9" rpy="0 0 0"/>
+  </joint>
+</robot>

scene/grasp-box/models/apriltag/urdf/grab_box_table.xacro

@@ -0,0 +1,263 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="standalone_apriltag_cube">
+  
+  <!-- 定义参数 -->
+  <xacro:property name="table_height" value="0.88" />  <!-- 桌子高度参数 -->
+  <xacro:property name="table2_height" value="0.88" />  <!-- 桌子高度参数 -->
+  <xacro:property name="table_thickness" value="0.04" />  <!-- 桌面厚度 -->
+  <xacro:property name="leg_radius" value="0.02" />  <!-- 桌腿半径 -->
+  
+  <!-- 第一个桌子位置参数 -->
+  <xacro:property name="table1_x" value="0.0" />
+  <xacro:property name="table1_y" value="2.0" />
+  <xacro:property name="table1_z" value="${table_height}" />
+  
+  <!-- 第二个桌子位置参数 -->
+  <xacro:property name="table2_x" value="0.0" />
+  <xacro:property name="table2_y" value="-0.9" />
+  <xacro:property name="table2_z" value="${table2_height}" />
+
+  <!-- 立方体位置参数/参数相对于桌子来说 -->
+  <xacro:property name="cube_x" value="0.0" />
+  <xacro:property name="cube_y" value="0.0" />
+  <xacro:property name="cube_z" value="${table_thickness/2 + 0.05}" />
+  
+  <!-- 引入apriltag -->
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag.xacro"/>
+  
+  <!-- 添加一个世界链接作为根链接 -->
+  <link name="world"/>
+  
+  <!-- 创建第一个桌子链接 -->
+  <link name="table1_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table1_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+  
+  <!-- 将世界链接与第一个桌子连接 -->
+  <joint name="world_to_table1_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table1_link"/>
+    <origin xyz="${table1_x} ${table1_y} ${table1_z}" rpy="0.0 0.0 1.5707963267948966"/>
+  </joint>
+  
+  <!-- 创建第二个桌子链接 -->
+  <link name="table2_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table2_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+
+  <!-- 将世界链接与第二个桌子连接 -->
+  <joint name="world_to_table2_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table2_link"/>
+    <origin xyz="${table2_x} ${table2_y} ${table2_z}" rpy="0.0 0.0 1.5707963267948966"/>
+  </joint>
+  
+  <!-- 创建第一个桌子的四条桌腿 -->
+  <xacro:macro name="table_leg" params="name x y height">
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${height}"/>
+        </geometry>
+        <material name="wood">
+          <color rgba="0.7 0.5 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.5"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
+      </inertial>
+    </link>
+    
+    <gazebo reference="${name}">
+      <material>Gazebo/Wood</material>
+      <mu1>0.5</mu1>
+      <mu2>0.5</mu2>
+    </gazebo>
+  </xacro:macro>
+  
+  <!-- 创建第一个桌子的桌腿 -->
+  <xacro:table_leg name="table1_leg1" x="0.15" y="0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg2" x="-0.15" y="0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg3" x="0.15" y="-0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg4" x="-0.15" y="-0.15" height="${table_height}"/>
+  
+  <!-- 创建第二个桌子的桌腿 -->
+  <xacro:table_leg name="table2_leg1" x="0.15" y="0.15" height="${table2_height}"/>
+  <xacro:table_leg name="table2_leg2" x="-0.15" y="0.15" height="${table2_height}"/>
+  <xacro:table_leg name="table2_leg3" x="0.15" y="-0.15" height="${table2_height}"/>
+  <xacro:table_leg name="table2_leg4" x="-0.15" y="-0.15" height="${table2_height}"/>
+
+  <!-- 连接第一个桌子的桌腿 -->
+  <joint name="table1_to_leg1_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg2_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg3_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg4_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 连接第二个桌子的桌腿 -->
+  <joint name="table2_to_leg1_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg1"/>
+    <origin xyz="0.15 0.15 ${-table2_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg2_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table2_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg3_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table2_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg4_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table2_height/2}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- 创建三个基础链接作为立方体的中心 -->
+  <link name="apriltag_box_base">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material>
+        <color rgba="0.8 0.8 0.8 0.5"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <link name="apriltag_shelf_base">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material>
+        <color rgba="0.8 0.8 0.8 0.5"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+
+  <!-- 将立方体连接到世界坐标系 -->
+  <joint name="world_to_box_tag_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="apriltag_box_base"/>
+    <origin xyz="-0.0005 0 0.02" rpy="0 0 1.5707963"/>
+  </joint>
+
+  <joint name="world_to_table_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="apriltag_shelf_base"/>
+    <origin xyz="-0.0005 0 0.02" rpy="0 0 -1.5707963"/> <!-- 绕z轴旋转270度 (4.712389弧度 = 270度) -->
+  </joint>
+
+  <xacro:apriltag_marker parent_link="apriltag_box_base" id="1">
+    <origin xyz="0 0.1 0.1" rpy="1.5708 1.5708 0"/>
+  </xacro:apriltag_marker>
+  
+  <xacro:apriltag_marker parent_link="apriltag_shelf_base" id="0">
+    <origin xyz="0 0.1 0.8" rpy="1.5708 1.5708 0"/>
+  </xacro:apriltag_marker>
+</robot> 

scene/grasp-box/models/apriltag/urdf/standalone_cube.xacro

@@ -0,0 +1,232 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="standalone_apriltag_cube">
+  
+  <!-- 定义参数 -->
+  <xacro:property name="table_height" value="0.8" />  <!-- 桌子高度参数 -->
+  <xacro:property name="table_thickness" value="0.04" />  <!-- 桌面厚度 -->
+  <xacro:property name="leg_radius" value="0.02" />  <!-- 桌腿半径 -->
+  
+  <!-- 第一个桌子位置参数 -->
+  <xacro:property name="table1_x" value="0.0" />
+  <xacro:property name="table1_y" value="-1.0" />
+  <xacro:property name="table1_z" value="${table_height}" />
+  
+  <!-- 第二个桌子位置参数 -->
+  <xacro:property name="table2_x" value="0.0" />
+  <xacro:property name="table2_y" value="1.0" />
+  <xacro:property name="table2_z" value="${table_height}" />
+  
+  <!-- 立方体位置参数/参数相对于桌子来说 -->
+  <xacro:property name="cube_x" value="0.0" />
+  <xacro:property name="cube_y" value="0.0" />
+  <xacro:property name="cube_z" value="${table_thickness/2 + 0.05}" />
+  
+  <!-- 引入apriltag -->
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag.xacro"/>
+  
+  <!-- 添加一个世界链接作为根链接 -->
+  <link name="world"/>
+  
+  <!-- 创建第一个桌子链接 -->
+  <link name="table1_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table1_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+  
+  <!-- 将世界链接与第一个桌子连接 -->
+  <joint name="world_to_table1_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table1_link"/>
+    <origin xyz="${table1_x} ${table1_y} ${table1_z}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 创建第二个桌子链接 -->
+  <link name="table2_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table2_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+  
+  <!-- 将世界链接与第二个桌子连接 -->
+  <joint name="world_to_table2_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table2_link"/>
+    <origin xyz="${table2_x} ${table2_y} ${table2_z}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 创建第一个桌子的四条桌腿 -->
+  <xacro:macro name="table_leg" params="name x y">
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${table_height}"/>
+        </geometry>
+        <material name="wood">
+          <color rgba="0.7 0.5 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${table_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.5"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
+      </inertial>
+    </link>
+    
+    <gazebo reference="${name}">
+      <material>Gazebo/Wood</material>
+      <mu1>0.5</mu1>
+      <mu2>0.5</mu2>
+    </gazebo>
+  </xacro:macro>
+  
+  <!-- 创建第一个桌子的桌腿 -->
+  <xacro:table_leg name="table1_leg1" x="0.15" y="0.15"/>
+  <xacro:table_leg name="table1_leg2" x="-0.15" y="0.15"/>
+  <xacro:table_leg name="table1_leg3" x="0.15" y="-0.15"/>
+  <xacro:table_leg name="table1_leg4" x="-0.15" y="-0.15"/>
+  
+  <!-- 创建第二个桌子的桌腿 -->
+  <xacro:table_leg name="table2_leg1" x="0.15" y="0.15"/>
+  <xacro:table_leg name="table2_leg2" x="-0.15" y="0.15"/>
+  <xacro:table_leg name="table2_leg3" x="0.15" y="-0.15"/>
+  <xacro:table_leg name="table2_leg4" x="-0.15" y="-0.15"/>
+  
+  <!-- 连接第一个桌子的桌腿 -->
+  <joint name="table1_to_leg1_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg2_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg3_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg4_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 连接第二个桌子的桌腿 -->
+  <joint name="table2_to_leg1_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg2_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg3_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg4_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 创建一个基础链接作为立方体的中心 -->
+  <link name="apriltag_cube_base">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material>
+        <color rgba="0.8 0.8 0.8 0.5"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+
+  <!-- 将立方体直接连接到世界坐标系 -->
+  <joint name="world_to_cube_joint" type="fixed">
+    <parent link="world"/>
+    <child link="apriltag_cube_base"/>
+    <origin xyz="0.0 -1.3 1.2" rpy="0 0 0"/>
+  </joint>
+
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag_cube.xacro"/>
+  <xacro:left_arm_apriltags parent_link="apriltag_cube_base"/>
+  <!-- 确保您已为这些ID创建了相应的材质文件，放在 kuavo_assets/models/apriltag/materials/ 目录下 -->
+</robot> 

scene/grasp-box/models/apriltag/urdf/table_apriltag_y.xacro

@@ -0,0 +1,262 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="standalone_apriltag_cube">
+  
+  <!-- 定义参数 -->
+  <xacro:property name="table_height" value="0.8" />  <!-- 桌子高度参数 -->
+  <xacro:property name="table_thickness" value="0.04" />  <!-- 桌面厚度 -->
+  <xacro:property name="leg_radius" value="0.02" />  <!-- 桌腿半径 -->
+  
+  <!-- 第一个桌子位置参数 -->
+  <xacro:property name="table1_x" value="0.0" />
+  <xacro:property name="table1_y" value="-1.8" />
+  <xacro:property name="table1_z" value="${table_height}" />
+  
+  <!-- 第二个桌子位置参数 -->
+  <xacro:property name="table2_x" value="0.0" />
+  <xacro:property name="table2_y" value="1.8" />
+  <xacro:property name="table2_z" value="${table_height}" />
+
+  <!-- 立方体位置参数/参数相对于桌子来说 -->
+  <xacro:property name="cube_x" value="0.0" />
+  <xacro:property name="cube_y" value="0.0" />
+  <xacro:property name="cube_z" value="${table_thickness/2 + 0.05}" />
+  
+  <!-- 引入apriltag -->
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag.xacro"/>
+  
+  <!-- 添加一个世界链接作为根链接 -->
+  <link name="world"/>
+  
+  <!-- 创建第一个桌子链接 -->
+  <link name="table1_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table1_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+  
+  <!-- 将世界链接与第一个桌子连接 -->
+  <joint name="world_to_table1_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table1_link"/>
+    <origin xyz="${table1_x} ${table1_y} ${table1_z}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 创建第二个桌子链接 -->
+  <link name="table2_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table2_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+
+  <!-- 将世界链接与第二个桌子连接 -->
+  <joint name="world_to_table2_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table2_link"/>
+    <origin xyz="${table2_x} ${table2_y} ${table2_z}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 创建第一个桌子的四条桌腿 -->
+  <xacro:macro name="table_leg" params="name x y">
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${table_height}"/>
+        </geometry>
+        <material name="wood">
+          <color rgba="0.7 0.5 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${table_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.5"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
+      </inertial>
+    </link>
+    
+    <gazebo reference="${name}">
+      <material>Gazebo/Wood</material>
+      <mu1>0.5</mu1>
+      <mu2>0.5</mu2>
+    </gazebo>
+  </xacro:macro>
+  
+  <!-- 创建第一个桌子的桌腿 -->
+  <xacro:table_leg name="table1_leg1" x="0.15" y="0.15"/>
+  <xacro:table_leg name="table1_leg2" x="-0.15" y="0.15"/>
+  <xacro:table_leg name="table1_leg3" x="0.15" y="-0.15"/>
+  <xacro:table_leg name="table1_leg4" x="-0.15" y="-0.15"/>
+  
+  <!-- 创建第二个桌子的桌腿 -->
+  <xacro:table_leg name="table2_leg1" x="0.15" y="0.15"/>
+  <xacro:table_leg name="table2_leg2" x="-0.15" y="0.15"/>
+  <xacro:table_leg name="table2_leg3" x="0.15" y="-0.15"/>
+  <xacro:table_leg name="table2_leg4" x="-0.15" y="-0.15"/>
+
+  <!-- 连接第一个桌子的桌腿 -->
+  <joint name="table1_to_leg1_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg2_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg3_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg4_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <!-- 连接第二个桌子的桌腿 -->
+  <joint name="table2_to_leg1_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg2_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg3_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table2_to_leg4_joint" type="fixed">
+    <parent link="table2_link"/>
+    <child link="table2_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- 创建三个基础链接作为立方体的中心 -->
+  <link name="apriltag_box_base">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material>
+        <color rgba="0.8 0.8 0.8 0.5"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <link name="apriltag_shelf_base">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material>
+        <color rgba="0.8 0.8 0.8 0.5"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+
+  <!-- 将立方体连接到世界坐标系 -->
+  <joint name="world_to_box_tag_joint" type="fixed">
+    <parent link="world"/>
+    <child link="apriltag_box_base"/>
+    <origin xyz="0.0 1.7 1.1" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="world_to_cube2_joint" type="fixed">
+    <parent link="world"/>
+    <child link="apriltag_shelf_base"/>
+    <origin xyz="0.0 -1.7 1.6" rpy="0 0 4.712389"/> <!-- 绕z轴旋转270度 (4.712389弧度 = 270度) -->
+  </joint>
+
+  <xacro:apriltag_marker parent_link="apriltag_box_base" id="1">
+    <origin xyz="0 -0.055 -0.165" rpy="1.5708 -1.5708 0"/>
+  </xacro:apriltag_marker>
+  
+  <xacro:apriltag_marker parent_link="apriltag_shelf_base" id="0">
+    <origin xyz="0.055 0 -0.165" rpy="0 1.5708 0"/>
+  </xacro:apriltag_marker>
+</robot> 

scene/grasp-box/models/box.xacro

@@ -0,0 +1,105 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="block">
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box/models/apriltag/urdf/apriltag.xacro"/>
+
+  <!-- ===== 参数定义 ===== -->
+  <xacro:arg name="box_length"   default="0.5"/>  <!-- x -->
+  <xacro:arg name="box_width"    default="0.3"/>  <!-- y -->
+  <xacro:arg name="box_height"   default="0.2"/>  <!-- z -->
+  <xacro:arg name="box_mass"     default="1.0"/>
+  <xacro:arg name="tag_id"     default="0"/>
+
+  <!-- ===== 派生参数计算 ===== -->
+  <!-- 从参数创建属性 -->
+  <xacro:property name="box_length" value="$(arg box_length)"/>
+  <xacro:property name="box_width" value="$(arg box_width)"/>
+  <xacro:property name="box_height" value="$(arg box_height)"/>
+  <xacro:property name="box_mass" value="$(arg box_mass)"/>
+
+  <!-- 耳朵长度：箱子宽度 - 0.05 -->
+  <xacro:property name="ear_length" value="${box_width - 0.05}"/>
+  <!-- 耳朵偏移：箱子长度的一半 + 0.025 -->
+  <xacro:property name="ear_offset" value="${box_length/2 + 0.025}"/>
+  <xacro:property name="ear_width" value="0.1"/>
+  <xacro:property name="ear_thickness" value="0.025"/>
+
+  <!-- 惯性矩阵计算 -->
+  <xacro:property name="ixx" value="${(1.0/12.0) * box_mass * (box_height*box_height + box_width*box_width)}"/>
+  <xacro:property name="iyy" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_height*box_height)}"/>
+  <xacro:property name="izz" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_width*box_width)}"/>
+
+  <!-- ===== 耳朵宏定义 ===== -->
+  <!-- 参数：
+       side_offset：左右位置（正值右耳，负值左耳）
+  -->
+  <xacro:macro name="ear" params="side_offset">
+    <visual>
+      <geometry>
+        <box size="${ear_width} ${ear_length} ${ear_thickness}"/>
+      </geometry>
+      <origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="${ear_width} ${ear_length} ${ear_thickness}"/>
+      </geometry>
+      <origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
+      <surface>
+        <contact>
+          <max_contacts>10</max_contacts>
+          <ode>
+            <kp>1e6</kp>
+            <kd>100</kd>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+  </xacro:macro>
+
+  <!-- 主体 -->
+  <link name="base_link">
+    <!-- 主体视觉 -->
+    <visual>
+      <geometry>
+        <box size="${box_length} ${box_width} ${box_height}"/>
+      </geometry>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <material name="red">
+        <color rgba="1 0 0 1"/>
+      </material>
+    </visual>
+    <!-- 主体碰撞 -->
+    <collision>
+      <geometry>
+        <box size="${box_length} ${box_width} ${box_height}"/>
+      </geometry>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <surface>
+        <contact>
+          <max_contacts>10</max_contacts>
+          <ode>
+            <kp>1e6</kp>
+            <kd>100</kd>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+
+    <!-- 左耳（调用宏） -->
+    <xacro:ear side_offset="-${ear_offset}"/>
+    <!-- 右耳（调用宏） -->
+    <xacro:ear side_offset="${ear_offset}"/>
+
+    <!-- 质量惯性（自动计算） -->
+    <inertial>
+      <mass value="${box_mass}"/>
+      <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="0" iyz="0" ixz="0"/>
+    </inertial>
+  </link>
+
+<xacro:apriltag_marker parent_link="base_link" id="$(arg tag_id)">
+    <origin xyz="0 -${box_width/2 + 0.001} 0" rpy="-${pi/2} 0 0"/>
+</xacro:apriltag_marker>
+
+</robot>

scene/grasp-box/models/fixed_tag.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="apriltag_complete">
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box/models/apriltag/urdf/apriltag.xacro"/>
+  <xacro:arg name="tag_id" default="0"/>
+  
+  <link name="base_link">
+    <inertial>
+      <mass value="1e9"/>
+      <inertia ixx="1e9" ixy="0" ixz="0" iyy="1e9" iyz="0" izz="1e9"/>
+    </inertial>
+  </link>
+
+  <xacro:apriltag_marker parent_link="base_link" id="$(arg tag_id)">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </xacro:apriltag_marker>
+
+  <link name="world"/>
+  <joint name="base_link_fixed_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+
+
+</robot>

scene/grasp-box/models/table.xacro

@@ -0,0 +1,128 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="standalone_no_apriltag_table">
+  <xacro:arg name="x" default="0"/>
+  <xacro:arg name="y" default="0"/>
+  <xacro:arg name="height" default="0"/>
+
+  <!-- 定义参数 -->
+  <xacro:property name="table_height" value="$(arg height)" />  <!-- 桌子高度参数 -->
+  <xacro:property name="table_thickness" value="0.04" />  <!-- 桌面厚度 -->
+  <xacro:property name="leg_radius" value="0.02" />  <!-- 桌腿半径 -->
+  
+  <!-- 第一个桌子位置参数 -->
+  <xacro:property name="table1_x" value="$(arg x)" />
+  <xacro:property name="table1_y" value="$(arg y)" />
+  <xacro:property name="table1_z" value="${table_height}" />
+  
+  <!-- 立方体位置参数/参数相对于桌子来说 -->
+  <xacro:property name="cube_x" value="0.0" />
+  <xacro:property name="cube_y" value="0.0" />
+  <xacro:property name="cube_z" value="${table_thickness/2 + 0.05}" />
+  
+  <!-- 引入apriltag -->
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box/models/apriltag/urdf/apriltag.xacro"/>
+  
+  <!-- 添加一个世界链接作为根链接 -->
+  <link name="world"/>
+  
+  <!-- 创建第一个桌子链接 -->
+  <link name="table1_link">
+    <visual>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+      <material name="wood">
+        <color rgba="0.8 0.6 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.4 0.4 ${table_thickness}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="5.0"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  
+  <!-- 添加Gazebo物理属性 -->
+  <gazebo reference="table1_link">
+    <material>Gazebo/Wood</material>
+    <mu1>0.5</mu1>
+    <mu2>0.5</mu2>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <minDepth>0.001</minDepth>
+    <maxVel>1.0</maxVel>
+    <maxContacts>20</maxContacts>
+  </gazebo>
+  
+  <!-- 将世界链接与第一个桌子连接 -->
+  <joint name="world_to_table1_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table1_link"/>
+    <origin xyz="${table1_x} ${table1_y} ${table1_z}" rpy="0.0 0.0 1.5707963267948966"/>
+  </joint>
+  
+  <!-- 创建第一个桌子的四条桌腿 -->
+  <xacro:macro name="table_leg" params="name x y height">
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${height}"/>
+        </geometry>
+        <material name="wood">
+          <color rgba="0.7 0.5 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder radius="${leg_radius}" length="${height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.5"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
+      </inertial>
+    </link>
+    
+    <gazebo reference="${name}">
+      <material>Gazebo/Wood</material>
+      <mu1>0.5</mu1>
+      <mu2>0.5</mu2>
+    </gazebo>
+  </xacro:macro>
+  
+  <!-- 创建第一个桌子的桌腿 -->
+  <xacro:table_leg name="table1_leg1" x="0.15" y="0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg2" x="-0.15" y="0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg3" x="0.15" y="-0.15" height="${table_height}"/>
+  <xacro:table_leg name="table1_leg4" x="-0.15" y="-0.15" height="${table_height}"/>
+  
+  <!-- 连接第一个桌子的桌腿 -->
+  <joint name="table1_to_leg1_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg1"/>
+    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg2_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg2"/>
+    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg3_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg3"/>
+    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+  <joint name="table1_to_leg4_joint" type="fixed">
+    <parent link="table1_link"/>
+    <child link="table1_leg4"/>
+    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
+  </joint>
+  
+</robot>

scene/grasp-box/scripts/gen_models.py

@@ -0,0 +1,81 @@
+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)

scene_plugin.py

@@ -0,0 +1,16 @@
+class ScenePlugin:
+    """
+    场景自定义模型的处理。
+    """
+
+    def __init__(self, scene_name):
+        self.scene_name = scene_name
+
+    def post_scene_model_spawn(self):
+        raise NotImplementedError()
+
+    def post_scene_model_state(self):
+        raise NotImplementedError()
+
+    def get_scene_model_state(self):
+        raise NotImplementedError()