feat: 正确终止服务器（疑似）

http_server.py

@@ -95,7 +95,7 @@ class GazeboSimHttpServer(HttpRPCServer):
         # 启动roscore
         self.roscore_proc = subprocess.Popen(
             ['roscore'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
-        time.sleep(2)
+        time.sleep(5)
         rospy.init_node('gazebo_world_manager', anonymous=True)
 
         # 添加接口
@@ -143,7 +143,6 @@ class GazeboSimHttpServer(HttpRPCServer):
                 # 关闭现有场景
                 if self.scene_mgr.running_scene:
                     self.shut_scene()
-                    time.sleep(25)
                 # 启动场景
                 self.scene_mgr.start_scene(id)
                 self.model_mgr = ModelManager()
@@ -163,11 +162,8 @@ class GazeboSimHttpServer(HttpRPCServer):
             try:
                 if not self.scene_mgr.running_scene:
                     raise RuntimeError('No scene is running')
-
-                def shutt():
-                    self.scene_mgr.shut_scene_truly()
-
-                threading.Thread(target=shutt, daemon=True).start()
+                self.scene_mgr.shut_scene()
+                time.sleep(5)
                 self.model_mgr = None
             except Exception as e:
                 ret['code'] = 400
@@ -263,6 +259,22 @@ class GazeboSimHttpServer(HttpRPCServer):
             print('Server remove model done.')
             return jsonify(ret)
 
+    def shutdown(self):
+        """
+        关闭所有资源。
+        """
+        # 1. 关闭场景（如果正在运行）
+        if self.scene_mgr.running_scene:
+            self.shut_scene()
+        # 2. 关闭 HeartbeatServer
+        self.heartbeat_server.stop()
+        # 3. 关闭 roscore 进程
+        if self.roscore_proc:
+            self.roscore_proc.terminate()
+            self.roscore_proc.wait()
+        # 4. 关闭 Flask HTTP 服务器
+        super().stop()
+        print("GazeboSimHttpServer stopped.")
 
 if __name__ == '__main__':
     server = GazeboSimHttpServer()

main.py

@@ -6,8 +6,7 @@ from http_server import GazeboSimHttpServer
 server = GazeboSimHttpServer()
 
 def signal_handler(sig, frame):
-    print('Exiting...')
-    server.stop()
+    server.shutdown()
     sys.exit(0)
 
 signal.signal(signal.SIGINT, signal_handler)

model_mgr/manager.py

@@ -1,4 +1,5 @@
 import threading
+import time
 from typing import List
 
 from gazebo_ctrl import GazeboROSController
@@ -17,9 +18,16 @@ class ModelManager:
         self.rlock = threading.RLock()
 
     def load_models(self, pmodels: List[PreloadModel]):
+        static_types = [ModelType.TABLE, ModelType.TAG]
         with self.rlock:
+            # 性能平凡的机器上，需要先加载静物，再加载动物
             for pmod in pmodels:
-                self.add_model(pmod.model, pmod.gen)
+                if pmod.model.obj_type in static_types:
+                    self.add_model(pmod.model, pmod.gen)
+            time.sleep(5)
+            for pmod in pmodels:
+                if pmod.model.obj_type not in static_types:
+                    self.add_model(pmod.model, pmod.gen)
 
     def add_model(self, model: Model, add_sim: bool = True):
         with self.rlock:
@@ -81,7 +89,7 @@ class ModelManager:
         with self.rlock:
             # 视觉元素tag.id、box.size、table.height无法动态修改，于是我们选择先删除再添加
             if model.obj_type == ModelType.ROBOT:
-                raise ValueError(f'Cannot remove robot model {model.id} {model.name}')
+                raise ValueError(f'Cannot modify robot model {model.id} {model.name}')
             self.remove_model(model.id)
             self.add_model(model)
 

model_mgr/model_types.py

@@ -163,7 +163,8 @@ class Model:
         self.id = data.get("id", self.id)
         self.pose.overwrite_from_dict(data.get("pose", {}))
         self.description = data.get("description", self.description)
-        self.ability_code = data.get("ability_code", self.ability_code)
+        if "ability_code" in data:
+            self.ability_code = data["ability_code"]
         self.tag_id = data.get("tag_id", self.tag_id)
         self.mass = data.get("mass", self.mass)
         self.size.overwrite_from_dict(data.get("size", {}))

scene/grasp_box/config.json

@@ -5,7 +5,8 @@
             "id": 1,
             "name": "biped_s42",
             "obj_type": 1,
-            "description": "a biped robot model"
+            "description": "a biped robot model",
+            "ability_code": ["move", "both_grab", "both_place", "identify", "turn", "head"]
         },
         {
             "gen": true,

scene_mgr/manager.py

@@ -89,21 +89,9 @@ class SceneManager:
         self.pid_file = None
         self.sim_process.terminate()
         print(f'killed launch PID: {self.sim_process.pid}')
-        # 杀死gazebo和rviz
-        subprocess.Popen(['pkill', '-15', '-f', 'gzserver'])
-        subprocess.Popen(['pkill', '-15', '-f', 'gzclient'])
-        subprocess.Popen(['pkill', '-15', '-f', 'rviz'])
-        self.sim_process = None
+        self.kill_all_processes()
         self.running_scene = None
 
-    def shut_scene_truly(self):
-        id = self.running_scene
-        self.shut_scene()
-        time.sleep(2)
-        self.start_scene(id)
-        time.sleep(15)
-        self.shut_scene()
-
     def run_script(self, name):
         if not self.running_scene:
             raise RuntimeError('no scene running')
@@ -113,6 +101,28 @@ class SceneManager:
             raise ValueError(f'script {name} not found')
         subprocess.Popen(['bash', script_path])
 
+    def kill_all_processes(self):
+        try:
+            # 获取所有进程
+            output = subprocess.check_output(["ps", "aux"]).decode()
+
+            # 过滤出目标进程
+            target_pids = []
+            for line in output.splitlines():
+                # 排除包含"gazebo_world_manager"的进程
+                if any(x in line for x in ["ros", "rviz", "gazebo"]) and "grep" not in line and "gazebo_world_manager" not in line:
+                    target_pids.append(line.split()[1])  # 提取PID
+
+            if target_pids:
+                # 批量kill
+                subprocess.run(["kill", "-9"] + target_pids)
+                print(f"Killed PIDs: {', '.join(target_pids)}")
+            else:
+                print("No matching processes running.")
+
+        except subprocess.CalledProcessError as e:
+            print(f"Error: {e}")
+
 
 if __name__ == '__main__':
     mgr = SceneManager()

test.py

@@ -14,12 +14,5 @@ from model_mgr.manager import ModelManager
 from scene_mgr.manager import SceneManager
 
 scene_mgr = SceneManager()
-model_mgr = ModelManager()
-
-model_mgr.load_models(scene_mgr.scene_id_dict[1].preload_models)
-models = model_mgr.get_all_models()
-print([x.to_dict() for x in models])
-time.sleep(30)
-for model in models:
-    if model.name != 'biped_s42':
-        model_mgr.remove_model(model.id)
+pr = scene_mgr.scene_id_dict[1].preload_models
+print(f'preload models: {[m.model.to_dict() for m in pr]}')