feat: 实现程序、仿真控制，定制模型添加、查询

1.txt

@@ -0,0 +1,29 @@
+curl --location --request GET 'http://127.0.0.1:12300/simulation/start?scene=grasp-box'
+
+curl --location --request GET 'http://127.0.0.1:12300/simulation/shutdown'
+
+curl --location --request POST 'http://127.0.0.1:12300/grasp-box/model/spawn' \
+--header 'Content-Type: application/json' \
+--data-raw '{
+    "name": "box10",
+    "type": "box",
+    "pose": {
+        "position": {
+            "x": 2,
+            "y": 2,
+            "z": 1
+        },
+        "orientation": {
+            "roll": 0,
+            "pitch": 0,
+            "yaw": 0
+        }
+    },
+    "tag_id": 9,
+    "mass": 1,
+    "size": {
+        "l": 0.3,
+        "h": 0.2,
+        "w": 0.2
+    }
+}'

gazebo_ctrl.py

@@ -1,13 +1,16 @@
 import rospy
-from gazebo_msgs.srv import (GetLinkProperties, GetModelProperties,
-                             GetModelPropertiesResponse, GetModelState,
-                             GetModelStateResponse, GetWorldProperties)
+from gazebo_msgs.srv import (DeleteModel, GetLinkProperties,
+                             GetModelProperties, GetModelPropertiesResponse,
+                             GetModelState, GetModelStateResponse,
+                             GetWorldProperties)
 from std_srvs.srv import Empty
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
 
 
 class GazeboROSController:
     def __init__(self):
-        rospy.init_node('gazebo_ros_controller', anonymous=True)
+        if not rospy.core.is_initialized():
+            rospy.init_node('gazebo_world_manager', anonymous=True, disable_signals=True)
         self.model_info = None
 
     def get_model_names(self) -> list:
@@ -78,6 +81,13 @@ class GazeboROSController:
             return None
 
     def format_model_state(self, state: GetModelStateResponse) -> dict:
+        # 四元数转换为欧拉角
+        roll, pitch, yaw = euler_from_quaternion((
+            state.pose.orientation.x,
+            state.pose.orientation.y,
+            state.pose.orientation.z,
+            state.pose.orientation.w
+        ))
         return {
             'pose': {
                 'position': {
@@ -86,10 +96,9 @@ class GazeboROSController:
                     '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
+                    'roll': roll,
+                    'pitch': pitch,
+                    'yaw': yaw
                 }
             },
             'twist': {
@@ -123,25 +132,6 @@ class GazeboROSController:
             '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:
         """
         获取模型的状态。
@@ -160,6 +150,30 @@ class GazeboROSController:
             model_info.append(info)
         return model_info
 
+    def pause_physics(self):
+        rospy.wait_for_service('/gazebo/pause_physics')
+        pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
+        pause_physics()
+
+    def continue_physics(self):
+        rospy.wait_for_service('/gazebo/unpause_physics')
+        unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+        unpause_physics()
+
+    def reset_simulation(self):
+        rospy.wait_for_service('/gazebo/reset_simulation')
+        reset_simulation = rospy.ServiceProxy(
+            '/gazebo/reset_simulation', Empty)
+        reset_simulation()
+
+    def delete_model(self, name):
+        rospy.wait_for_service('/gazebo/delete_model')
+        delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
+        resp = delete_model(model_name=name)
+        if not resp.success:
+            raise RuntimeError(f"Failed to delete model {name}: {resp.status_message}")
+
+
 if __name__ == '__main__':
     controller = GazeboROSController()
     import json

http_server.py

@@ -4,8 +4,10 @@ import os
 import socket
 import subprocess
 import threading
+import time
 
 import flask
+import rospy
 import werkzeug.serving
 from flask import jsonify, request
 
@@ -82,7 +84,11 @@ class GazeboSimHttpServer(HttpRPCServer):
     def __init__(self):
         super().__init__()
         self.scene_mgr = scene_mgr.SceneManager()
-        self.gazebo_ctrl = gazebo_ctrl.GazeboROSController()
+        # 启动roscore
+        self.roscore_proc = subprocess.Popen(
+            ['roscore'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+        time.sleep(2)
+        rospy.init_node('gazebo_world_manager', anonymous=True)
 
         # Gazebo仿真/程序控制
         self.add_route('/simulation/scene', self.get_simulation_scene, ['GET'])
@@ -114,14 +120,20 @@ class GazeboSimHttpServer(HttpRPCServer):
     def get_simulation_scene(self):
         try:
             scenes = self.scene_mgr.get_scene()
+            # 下划线转横杠
+            scenes = [s.replace('_', '-') for s in scenes]
         except Exception as e:
             return str(e), 400
         return jsonify(scenes)
 
     def start_simulation(self):
         scene = request.args.get('scene', 'grasp-box')
+        # 横杠转下划线
+        scene = scene.replace('-', '_')
         try:
             self.scene_mgr.start_scene(scene)
+            self.gazebo_ctrl = gazebo_ctrl.GazeboROSController()
+            self.scene_name = scene
         except Exception as e:
             return str(e), 400
         return 'OK', 200
@@ -129,6 +141,8 @@ class GazeboSimHttpServer(HttpRPCServer):
     def shutdown_simulation(self):
         try:
             self.scene_mgr.shut_scene()
+            self.gazebo_ctrl = None
+            self.scene_name = None
         except Exception as e:
             return str(e), 400
         return 'OK', 200
@@ -143,16 +157,24 @@ class GazeboSimHttpServer(HttpRPCServer):
 
     def continue_physics(self):
         try:
-            self.gazebo_ctr???
+            self.gazebo_ctrl.continue_physics()
         except Exception as e:
             return str(e), 400
         return 'OK', 200
 
     def pause_physics(self):
-        return jsonify({'status': 'physics paused'})
+        try:
+            self.gazebo_ctrl.pause_physics()
+        except Exception as e:
+            return str(e), 400
+        return 'OK', 200
 
     def reset_physics(self):
-        return jsonify({'status': 'physics reset'})
+        try:
+            self.gazebo_ctrl.reset_simulation()
+        except Exception as e:
+            return str(e), 400
+        return 'OK', 200
 
     def get_model_state(self):
         names = request.args.getlist('names') or []
@@ -168,30 +190,42 @@ class GazeboSimHttpServer(HttpRPCServer):
         data = request.get_json(silent=True)
         required = ['pose', 'name', 'xml']
         if not data or not all(k in data for k in required):
-            return jsonify({'error': f'Missing required fields {required}'}), 400
-        return jsonify({'status': 'model spawned', 'data': data})
+            return f'Missing required fields {required}', 400
+        return 'model not spawned', 200
 
     def delete_model(self):
         data = request.get_json(silent=True)
         if not data or 'name' not in data:
-            return jsonify({'error': 'Missing required field name'}), 400
-        return jsonify({'status': 'model deleted', 'name': data['name']})
+            return 'Missing required field name', 400
+        try:
+            self.gazebo_ctrl.delete_model(data['name'])
+        except Exception as e:
+            return str(e), 400
+        return 'OK', 200
 
     def get_scene_model_state(self, scene):
+        # 横杠转下划线
+        scene = scene.replace('-', '_')
         if scene != self.scene_name:
-            return jsonify({'error': f'running {self.scene_name} but try to access {scene}'}), 400
+            return f'running {self.scene_name} but try to access {scene}', 400
         return self.scene_mgr.scene_plugin.get_scene_model_state()
 
     def set_scene_model_state(self, scene):
+        # 横杠转下划线
+        scene = scene.replace('-', '_')
         if scene != self.scene_name:
-            return jsonify({'error': f'running {self.scene_name} but try to access {scene}'}), 400
+            return f'running {self.scene_name} but try to access {scene}', 400
         return self.scene_mgr.scene_plugin.post_scene_model_state()
 
     def spawn_scene_model(self, scene):
+        print(111)
+        # 横杠转下划线
+        scene = scene.replace('-', '_')
         if scene != self.scene_name:
-            return jsonify({'error': f'running {self.scene_name} but try to access {scene}'}), 400
+            return f'running {self.scene_name} but try to access {scene}', 400
         return self.scene_mgr.scene_plugin.post_scene_model_spawn()
 
+
 if __name__ == '__main__':
     server = GazeboSimHttpServer()
-    server.start(hostname='0.0.0.0', port=12300)
+    server.start(hostname='0.0.0.0', port=12300)

main.py

@@ -1,5 +1,16 @@
+import signal
+import sys
+
 from http_server import GazeboSimHttpServer
 
+server = GazeboSimHttpServer()
+
+def signal_handler(sig, frame):
+    print('Exiting...')
+    server.stop()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, signal_handler)
+
 if __name__ == '__main__':
-    server = GazeboSimHttpServer()
     server.start(hostname='0.0.0.0', port=12300)

scene/grasp-box/plugin.py

@@ -1,15 +0,0 @@
-import scene_plugin
-
-
-class Plugin(scene_plugin.ScenePluginBase):
-    def __init__(self, scene_name):
-        super().__init__(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()

scene/grasp-box/scripts/grasp_box_example.sh

@@ -1,3 +0,0 @@
-source ~/kuavo-ros-opensource/devel/setup.bash
-cd ~/gazebo_world_manager/scene/grasp-box/scripts
-python3 grasp_box_example.py --sim

scene/grasp_box/gen_models.py

@@ -9,7 +9,8 @@ from tf.transformations import quaternion_from_euler
 
 class GraspModelGenerator:
     def __init__(self):
-        rospy.init_node('gazebo_world_manager', anonymous=True)
+        if not rospy.core.is_initialized():
+            rospy.init_node('gazebo_world_manager', anonymous=True, disable_signals=True)
         self.spawn_service = rospy.ServiceProxy(
             '/gazebo/spawn_urdf_model', SpawnModel)
         ros_package_path = os.environ.get('ROS_PACKAGE_PATH', '')
@@ -37,22 +38,23 @@ class GraspModelGenerator:
         except subprocess.CalledProcessError as e:
             rospy.logerr(f"Failed to convert xacro to urdf: {e}")
             return False, f"Failed to convert xacro to urdf: {e}"
+        return True, f"Model {model_name} spawned successfully"
 
-    def generate_table(self, name, x=0, y=0, height=1):
-        xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/table.xacro"
+    def generate_table(self, name, x=0, y=0, height=1, roll=0, pitch=0, yaw=0):
+        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))
+            position=Point(x=x, y=y, z=height),
+            orientation=Quaternion(*quaternion_from_euler(roll, pitch, yaw))
         )
         return 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"
+        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')
@@ -65,7 +67,8 @@ class GraspModelGenerator:
 
     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"
+        print(f'{name}:x={x},y={y},z={z}')
+        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')
@@ -79,4 +82,6 @@ class GraspModelGenerator:
 
 if __name__ == '__main__':
     generator = GraspModelGenerator()
-    generator.generate_apriltag("tg0", 0, 2, 0, 1.1, 0, 1.57, 0)
+    # generator.generate_apriltag("tg0", 0, 2, 0, 1.1, 0, 1.57, 0)
+    generator.generate_table("tb0", x=2, y=1, height=0.8)
+    # generator.generate_box_with_apriltag("bx0", x=2, z=1)

scene/grasp_box/launch.sh

@@ -1,5 +1,7 @@
 #!/bin/bash
 
+PID_FILE=$1
+
 # 检查 Python3 是否安装
 if ! command -v python3 &> /dev/null; then
     echo -e "\033[31m错误：未找到 Python3，请确保已安装 Python3\033[0m"
@@ -12,6 +14,23 @@ if ! command -v roslaunch &> /dev/null; then
     exit 1
 fi
 
+# 清理旧的 PID 文件（如果存在）
+if [ -f "$PID_FILE" ]; then
+    rm "$PID_FILE"
+    echo -e "\033[33m已删除旧的 PID 文件: $PID_FILE\033[0m"
+fi
+
+# 捕获 Ctrl+C 信号，清理后台进程
+trap 'echo -e "\033[31m接收到 Ctrl+C，清理后台进程...\033[0m"; \
+      if [ -f "$PID_FILE" ]; then \
+          while read -r pid; do \
+              kill $pid 2>/dev/null && echo -e "\033[32m已终止进程 PID: $pid\033[0m"; \
+          done < "$PID_FILE"; \
+          rm "$PID_FILE"; \
+          echo -e "\033[32m已删除 PID 文件: $PID_FILE\033[0m"; \
+      fi; \
+      exit 0' SIGINT
+
 # 执行 kuavo_tf2_web_republisher
 echo -e "\033[32m启动 kuavo_tf2_web_republisher...\033[0m"
 cd ~/kuavo_ros_application && source devel/setup.bash && roslaunch kuavo_tf2_web_republisher start_websocket_server.launch &
@@ -56,8 +75,8 @@ else
     exit 1
 fi
 
-# 清理后台进程
-echo -e "\033[32m清理后台 ROS 进程...\033[0m"
-kill $WEB_PID $GAZEBO_PID $STRATEGIES_PID 2>/dev/null
-rm "$PID_FILE" 2>/dev/null
-echo -e "\033[32m已删除 PID 文件: $PID_FILE\033[0m"
+# # 清理后台进程
+# echo -e "\033[32m清理后台 ROS 进程...\033[0m"
+# kill $WEB_PID $GAZEBO_PID $STRATEGIES_PID 2>/dev/null
+# rm "$PID_FILE" 2>/dev/null
+# echo -e "\033[32m已删除 PID 文件: $PID_FILE\033[0m"

scene/grasp_box/models/apriltag/urdf/apriltag.xacro

@@ -27,7 +27,7 @@
       <visual>
         <material>
           <script>
-            <uri>file://$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/materials/apriltag_${id}.material</uri>
+            <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">

scene/grasp_box/models/apriltag/urdf/apriltag_cube.xacro

@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
   <!-- 引入apriltag -->
-  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box//models/apriltag/urdf/apriltag.xacro"/>
+  <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">

scene/grasp_box/models/apriltag/urdf/grab_box_table.xacro

@@ -23,7 +23,7 @@
   <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"/>
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp_box//models/apriltag/urdf/apriltag.xacro"/>
   
   <!-- 添加一个世界链接作为根链接 -->
   <link name="world"/>

scene/grasp_box/models/apriltag/urdf/standalone_cube.xacro

@@ -22,7 +22,7 @@
   <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"/>
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp_box//models/apriltag/urdf/apriltag.xacro"/>
   
   <!-- 添加一个世界链接作为根链接 -->
   <link name="world"/>
@@ -226,7 +226,7 @@
     <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: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

@@ -22,7 +22,7 @@
   <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"/>
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp_box//models/apriltag/urdf/apriltag.xacro"/>
   
   <!-- 添加一个世界链接作为根链接 -->
   <link name="world"/>

scene/grasp_box/models/box.xacro

@@ -1,6 +1,6 @@
 <?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:include filename="$(find gazebo_world_manager)/scene/grasp_box/models/apriltag/urdf/apriltag.xacro"/>
 
   <!-- ===== 参数定义 ===== -->
   <xacro:arg name="box_length"   default="0.5"/>  <!-- x -->

scene/grasp_box/models/fixed_tag.xacro

@@ -1,6 +1,6 @@
 <?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: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">

scene/grasp_box/models/table.xacro

@@ -20,7 +20,7 @@
   <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"/>
+  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp_box/models/apriltag/urdf/apriltag.xacro"/>
   
   <!-- 添加一个世界链接作为根链接 -->
   <link name="world"/>
@@ -62,7 +62,7 @@
   <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"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
   </joint>
   
   <!-- 创建第一个桌子的四条桌腿 -->

scene/grasp_box/pids.txt

@@ -0,0 +1,3 @@
+23376
+23579
+24764

scene/grasp_box/plugin.py

@@ -0,0 +1,175 @@
+import dataclasses
+
+from flask import jsonify, request
+
+import gazebo_ctrl
+import scene_plugin
+
+from .gen_models import GraspModelGenerator
+
+
+@dataclasses.dataclass
+class BoxModel:
+    length: float
+    weight: float
+    height: float
+    tag_id: int
+    mass: float
+    type: str = 'box'
+
+
+@dataclasses.dataclass
+class TableModel:
+    height: float
+    type: str = 'table'
+
+
+@dataclasses.dataclass
+class TagModel:
+    tag_id: int
+    type: str = 'tag'
+
+
+class Plugin(scene_plugin.ScenePluginBase):
+    def __init__(self, scene_name):
+        super().__init__(scene_name)
+        self.model_dict = {}
+        self.spawner = GraspModelGenerator()
+        self.gz_ctrl = gazebo_ctrl.GazeboROSController()
+
+    def post_scene_model_spawn(self):
+        print(222)
+        try:
+            # 解析参数
+            data = request.get_json(force=True)
+            name = data.get('name')
+            type_ = data.get('type')
+            pose = data.get('pose')
+            position = pose.get('position')
+            orientation = pose.get('orientation')
+            tag_id = data.get('tag_id', 0)
+            mass = data.get('mass', 1)
+            size = data.get('size')
+            if not name or not type_ or pose is None:
+                raise ValueError('name, type, and pose are required')
+            # 生成模型
+            if type_ == 'box':
+                if not size or not all(k in size for k in ('l', 'h', 'w')):
+                    raise ValueError('size with l, h, w is required for box')
+                model = BoxModel(
+                    length=size['l'],
+                    height=size['h'],
+                    weight=size['w'],
+                    tag_id=tag_id,
+                    mass=mass
+                )
+                ret, msg = self.spawner.generate_box_with_apriltag(
+                    name,
+                    l=size['l'],
+                    w=size['w'],
+                    h=size['h'],
+                    mass=mass,
+                    tag_id=tag_id,
+                    x=position.get('x', 0),
+                    y=position.get('y', 0),
+                    z=position.get('z', 0),
+                    roll=orientation.get('roll', 0),
+                    pitch=orientation.get('pitch', 0),
+                    yaw=orientation.get('yaw', 0)
+                )
+                if ret == False:
+                    raise RuntimeError(msg)
+            elif type_ == 'table':
+                model = TableModel(
+                    height=pose.get('z', 1)
+                )
+                self.spawner.generate_table(
+                    name,
+                    x=position.get('x', 0),
+                    y=position.get('y', 0),
+                    height=position.get('z', 0),
+                    roll=orientation.get('roll', 0),
+                    pitch=orientation.get('pitch', 0),
+                    yaw=orientation.get('yaw', 0)
+                )
+            elif type_ == 'tag':
+                model = TagModel(
+                    tag_id=tag_id
+                )
+                self.spawner.generate_apriltag(
+                    name,
+                    tag_id=tag_id,
+                    x=position.get('x', 0),
+                    y=position.get('y', 0),
+                    z=position.get('z', 0),
+                    roll=orientation.get('roll', 0),
+                    pitch=orientation.get('pitch', 0),
+                    yaw=orientation.get('yaw', 0)
+                )
+            else:
+                raise ValueError('Invalid type')
+            # 注册模型
+            self.model_dict[name] = model
+        except Exception as e:
+            print(str(e))
+            return str(e), 400
+        return 'OK', 200
+
+    def get_scene_model_state(self):
+        try:
+            res = self.gz_ctrl.get_model_state_f(self.model_dict.keys())
+            for item in res:
+                model = self.model_dict.get(item['name'])
+                if model.type == 'box':
+                    item['tag_id'] = model.tag_id
+                    item['mass'] = model.mass
+                    item['size'] = {'l': model.length, 'w': model.weight, 'h': model.height}
+                elif model.type == 'table':
+                    item['pose']['position']['z'] = model.height
+                elif model.type == 'tag':
+                    item['tag_id'] = model.tag_id
+            return jsonify(res), 200
+        except Exception as e:
+            return str(e), 400
+    
+    def post_scene_model_state(self):
+        try:
+            # 解析参数
+            data = request.get_json(force=True)
+            name = data.get('name')
+            type_ = data.get('type')
+            pose = data.get('pose')
+            position = pose.get('position')
+            orientation = pose.get('orientation')
+            tag_id = data.get('tag_id', 0)
+            mass = data.get('mass', 0)
+            size = data.get('size')
+            if not name or not type_ or pose is None:
+                raise ValueError('name, type, and pose are required')
+            # 修改模型
+            model = self.model_dict[name]
+            if model.type != type_:
+                raise ValueError(f'Cannot change model type from {self.model_dict[name].type} to {type_}')
+            # 修改pose
+            pass
+            # 修改独特属性
+            if type_ == 'box':
+                if mass:
+                    model.mass = mass
+                if tag_id:
+                    model.tag_id = tag_id
+            elif type_ == 'table':
+                height = position.get('z', 0)
+                if height:
+                    model.height = height
+            elif type_ == 'tag':
+                if tag_id:
+                    model.tag_id = tag_id
+            else:
+                raise ValueError('Invalid type')
+        except Exception as e:
+            return str(e), 400
+        return 'OK', 200
+
+if __name__ == '__main__':
+    pass

scene/grasp_box/scripts/grasp_box_example.sh

@@ -0,0 +1,3 @@
+source ~/kuavo-ros-opensource/devel/setup.bash
+cd ~/gazebo_world_manager/scene/grasp_box/scripts
+python3 grasp_box_example.py --sim

scene_mgr.py

@@ -1,6 +1,7 @@
 import dataclasses
 import importlib
 import os
+import signal
 import subprocess
 import threading
 
@@ -21,6 +22,7 @@ class SceneManager:
         self.sim_process = None
         self.sim_thread = None
         self.scene_plugin = None
+        self.load_scene()
 
     def load_scene(self):
         """
@@ -46,7 +48,7 @@ class SceneManager:
 
     def get_scene(self):
         return list(self.scene_dict.keys())
-    
+
     def get_scripts(self, name):
         return list(self.scene_dict[name].scripts.keys())
 
@@ -56,18 +58,34 @@ class SceneManager:
                 f'scene {self.running_scene} is already running')
         if name not in self.scene_dict:
             raise ValueError(f'scene {name} not found')
+        self.pid_file = os.path.join(
+            os.path.dirname(__file__), 'scene', name, 'pids.txt')
+        if os.path.exists(self.pid_file):
+            os.remove(self.pid_file)
+        self.sim_process = subprocess.Popen(
+            ['bash', self.scene_dict[name].launch_file, self.pid_file],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE
+        )
         scene_plugin_module = importlib.import_module(f'scene.{name}.plugin')
         self.scene_plugin = scene_plugin_module.Plugin(name)
         self.running_scene = name
-        self.sim_process = subprocess.Popen(
-                ['bash', self.scene_dict[name].launch_file],
-                stdout=subprocess.PIPE,
-                stderr=subprocess.PIPE
-            )
 
     def shut_scene(self):
-        # TODO: 并没有正确终止仿真系列进程
-        self.sim_process.terminate()
+        with open(self.pid_file, 'r') as f:
+            pids = [line.strip() for line in f if line.strip()]
+        for pid in pids:
+            try:
+                os.kill(int(pid), signal.SIGTERM)
+                print(f"killed PID: {pid}")
+            except ProcessLookupError:
+                print(f"process PID: {pid} already gone")
+            except ValueError:
+                print(f"invalid PID: {pid}")
+        # os.remove(self.pid_file)
+        self.pid_file = None
+        print(self.sim_process.returncode)
+        self.sim_process.kill()
         self.sim_process = None
         self.running_scene = None
         self.scene_plugin = None
@@ -75,19 +93,19 @@ class SceneManager:
     def run_script(self, name):
         if not self.running_scene:
             raise RuntimeError('no scene running')
-        script_path = self.scene_dict[self.running_scene].scripts.get(name, None)
+        script_path = self.scene_dict[self.running_scene].scripts.get(
+            name, None)
         if not script_path:
             raise ValueError(f'script {name} not found')
         subprocess.Popen(['bash', script_path])
 
+
 if __name__ == '__main__':
     mgr = SceneManager()
     mgr.load_scene()
     print(mgr.get_scene())
-    print(mgr.get_scripts('grasp-box'))
-    mgr.start_scene('grasp-box')
+    print(mgr.get_scripts('grasp_box'))
+    mgr.start_scene('grasp_box')
     import time
-    time.sleep(20)
-    mgr.run_script('grasp_box_example')
-    time.sleep(60)
+    time.sleep(40)
     mgr.shut_scene()