diff --git a/http_server.py b/http_server.py index 6e0971e..71b0a6e 100644 --- a/http_server.py +++ b/http_server.py @@ -12,6 +12,7 @@ from flask import jsonify, request import gazebo_ctrl from heartbeat import HeartbeatServer from model_mgr.manager import ModelManager +from model_mgr.model_types import Model from scene_mgr.manager import SceneManager @@ -86,8 +87,12 @@ class GazeboSimHttpServer(HttpRPCServer): # 模块与数据 self.rlock = threading.RLock() self.scene_mgr = SceneManager() - self.model_mgr = ModelManager() - self.heartbeat_server = HeartbeatServer() + self.model_mgr = None + + def llost(): + self.shut_scene() + self.heartbeat_server = HeartbeatServer(close_func=llost) + self.heartbeat_server.start() # 启动roscore self.roscore_proc = subprocess.Popen( ['roscore'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) @@ -95,47 +100,160 @@ class GazeboSimHttpServer(HttpRPCServer): rospy.init_node('gazebo_world_manager', anonymous=True) # 添加接口 - self._app.add_url_rule('/api/v1/scenes', view_func=self.query_scenes, methods=['GET']) - self._app.add_url_rule('/api/v1/scene/load', view_func=self.load_scene, methods=['GET']) - self._app.add_url_rule('/api/v1/scene/objects', view_func=self.get_scene_objects, methods=['GET']) - self._app.add_url_rule('/api/v1/scene/object', view_func=self.modify_scene_object, methods=['POST']) - self._app.add_url_rule('/api/v1/scene/object', view_func=self.add_scene_object, methods=['PUT']) - self._app.add_url_rule('/api/v1/scene/object', view_func=self.remove_scene_object, methods=['DELETE']) + self._app.add_url_rule( + '/api/v1/scenes', view_func=self.query_scenes, methods=['GET']) + self._app.add_url_rule('/api/v1/scene/load', + view_func=self.start_scene, methods=['GET']) + self._app.add_url_rule('/api/v1/scene/objects', + view_func=self.get_all_models, methods=['GET']) + self._app.add_url_rule( + '/api/v1/scene/object', view_func=self.modify_model, methods=['POST']) + self._app.add_url_rule('/api/v1/scene/object', + view_func=self.add_model, methods=['PUT']) + self._app.add_url_rule( + '/api/v1/scene/object', view_func=self.remove_model, methods=['DELETE']) + + print('Sever init done.') def query_scenes(self): # 查询场景 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + scs = self.scene_mgr.get_scenes() + data = [sc.to_dict for sc in scs] + ret['data'] = data + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server query scenes done.') + return jsonify(ret) def start_scene(self): # 启动场景 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + # 检查参数 + id = request.args.get('id', type=int, default=None) + if not id: + raise ValueError('Scene ID is required') + # 关闭现有场景 + if self.scene_mgr.running_scene: + self.shut_scene() + time.sleep(25) + # 启动场景 + self.scene_mgr.start_scene(id) + self.model_mgr = ModelManager() + time.sleep(30) + self.model_mgr.load_models( + self.scene_mgr.scene_id_dict[id].preload_models) + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server start scene done.') + return jsonify(ret) def shut_scene(self): # 关闭场景,仅供start_scene、心跳线程使用 with self.rlock: - pass + 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.model_mgr = None + except Exception as e: + print(str(e)) + print('Server shut scene done.') def get_all_models(self): # 获取所有模型 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + mods = self.model_mgr.get_all_models() + cnt = len(mods) + data = {} + data['total_count'] = cnt + data['list'] = [m.to_dict() for m in mods] + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server get all models done.') + return jsonify(ret) def modify_model(self): # 修改模型 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + # 解析参数 + info = request.get_json() + scene_id = info.get('scene_id', type=int, default=None) + if not scene_id or scene_id != self.scene_mgr.running_scene: + raise ValueError( + f'Scene {scene_id} does not match the running scene {self.scene_mgr.running_scene}') + object_id = info.get('object_id', type=int, default=None) + if not object_id: + raise ValueError('Object ID is required') + # 制作Model + info['id'] = object_id + model = self.model_mgr.get_model(object_id) + model.overwrite_from_dict(info) + # 添加模型 + self.model_mgr.modify_model(model) + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server modify model done.') + return jsonify(ret) def add_model(self): # 添加模型 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + # 解析参数 + info = request.get_json() + scene_id = info.get('scene_id', type=int, default=None) + if not scene_id or scene_id != self.scene_mgr.running_scene: + raise ValueError( + f'Scene {scene_id} does not match the running scene {self.scene_mgr.running_scene}') + # 制作Model,和modify的区别在于没有id + info['id'] = -1 + model = Model.from_dict(info) + # 添加模型 + self.model_mgr.add_model(model) + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server add model done.') + return jsonify(ret) def remove_model(self): # 删除模型 with self.rlock: - pass + ret = {'code': 200, 'message': ''} + try: + # 解析参数 + info = request.get_json() + scene_id = info.get('scene_id', type=int, default=None) + if not scene_id or scene_id != self.scene_mgr.running_scene: + raise ValueError( + f'Scene {scene_id} does not match the running scene {self.scene_mgr.running_scene}') + object_id = info.get('object_id', type=int, default=None) + # 删除模型 + self.model_mgr.remove_model(object_id) + except Exception as e: + ret['code'] = 400 + ret['message'] = str(e) + print('Server remove model done.') + return jsonify(ret) + if __name__ == '__main__': server = GazeboSimHttpServer() diff --git a/model_mgr/model_types.py b/model_mgr/model_types.py index 31e8970..b77a668 100644 --- a/model_mgr/model_types.py +++ b/model_mgr/model_types.py @@ -13,6 +13,11 @@ class Position: def to_dict(self): return {"x": self.x, "y": self.y, "z": self.z} + + def overwrite_from_dict(self, data): + self.x = data.get("x", self.x) + self.y = data.get("y", self.y) + self.z = data.get("z", self.z) @classmethod def from_dict(cls, data): @@ -32,6 +37,11 @@ class Orientation: def to_dict(self): return {"roll": self.roll, "pitch": self.pitch, "yaw": self.yaw} + def overwrite_from_dict(self, data): + self.roll = data.get("roll", self.roll) + self.pitch = data.get("pitch", self.pitch) + self.yaw = data.get("yaw", self.yaw) + @classmethod def from_dict(cls, data): return cls( @@ -52,6 +62,10 @@ class Pose: position: Position = dataclasses.field(default_factory=Position) orientation: Orientation = dataclasses.field(default_factory=Orientation) + def overwrite_from_dict(self, data): + self.position.overwrite_from_dict(data.get("position", {})) + self.orientation.overwrite_from_dict(data.get("orientation", {})) + def to_dict(self): return { "position": self.position.to_dict(), @@ -79,6 +93,11 @@ class Size: "height": self.height, } + def overwrite_from_dict(self, data): + self.length = data.get("length", self.length) + self.width = data.get("width", self.width) + self.height = data.get("height", self.height) + @classmethod def from_dict(cls, data): return cls( @@ -113,12 +132,17 @@ class Model: mass: int = 1 size: Size = dataclasses.field(default_factory=Size) - def __post_init__(self): + @classmethod + def gen_id(cls): global _nr_model_id + global _nr_model_id_lock + with _nr_model_id_lock: + _nr_model_id += 1 + return _nr_model_id + + def __post_init__(self): if self.id == -1: - with _nr_model_id_lock: - self.id = _nr_model_id - _nr_model_id += 1 + self.id = self.gen_id() def to_dict(self): return { @@ -133,12 +157,23 @@ class Model: "size": self.size.to_dict(), } + def overwrite_from_dict(self, data): + self.name = data.get("name", self.name) + self.obj_type = ModelType(data.get("obj_type", self.obj_type)) + 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) + 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", {})) + @classmethod def from_dict(cls, data): return cls( name=data.get("name", ""), obj_type=ModelType(data.get("obj_type", 0)), - id=data.get("id", -1), + id=data.get("id", cls.gen_id()), # 添加模型用 pose=Pose.from_dict(data.get("pose", {})), description=data.get("description", ""), ability_code=data.get("ability_code", []), diff --git a/scene_mgr/manager.py b/scene_mgr/manager.py index 9a4736d..22f892d 100644 --- a/scene_mgr/manager.py +++ b/scene_mgr/manager.py @@ -52,7 +52,7 @@ class SceneManager: self.scene_name_dict[name] = scene self.scene_id_dict[scene.id] = scene - def get_scene(self): + def get_scenes(self): return list(self.scene_name_dict.values()) def get_scripts(self, name):