diff --git a/gazebo_ctrl.py b/gazebo_ctrl.py index 3e8a02a..e194a4b 100644 --- a/gazebo_ctrl.py +++ b/gazebo_ctrl.py @@ -145,7 +145,7 @@ class GazeboROSController: """ if not model_names or len(model_names) == 0: model_names = self.get_model_names() - print(f"Getting info for models: {model_names}") + # print(f"Getting info for models: {model_names}") model_info = [] states = self.get_model_state(model_names) for name in model_names: diff --git a/http_server.py b/http_server.py index 71b0a6e..4f5cbc3 100644 --- a/http_server.py +++ b/http_server.py @@ -9,7 +9,6 @@ import rospy import werkzeug.serving 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 @@ -113,6 +112,9 @@ class GazeboSimHttpServer(HttpRPCServer): self._app.add_url_rule( '/api/v1/scene/object', view_func=self.remove_model, methods=['DELETE']) + self._app.add_url_rule('/api/v1/scene/shut', + view_func=self.shut_scene, methods=['GET']) + print('Sever init done.') def query_scenes(self): @@ -121,12 +123,12 @@ class GazeboSimHttpServer(HttpRPCServer): ret = {'code': 200, 'message': ''} try: scs = self.scene_mgr.get_scenes() - data = [sc.to_dict for sc in scs] + 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.') + print(f'Server query scenes done. {ret}') return jsonify(ret) def start_scene(self): @@ -155,8 +157,9 @@ class GazeboSimHttpServer(HttpRPCServer): return jsonify(ret) def shut_scene(self): - # 关闭场景,仅供start_scene、心跳线程使用 + # 关闭场景 with self.rlock: + ret = {'code': 200, 'message': ''} try: if not self.scene_mgr.running_scene: raise RuntimeError('No scene is running') @@ -167,8 +170,10 @@ class GazeboSimHttpServer(HttpRPCServer): threading.Thread(target=shutt, daemon=True).start() self.model_mgr = None except Exception as e: - print(str(e)) + ret['code'] = 400 + ret['message'] = str(e) print('Server shut scene done.') + return ret def get_all_models(self): # 获取所有模型 @@ -180,10 +185,11 @@ class GazeboSimHttpServer(HttpRPCServer): data = {} data['total_count'] = cnt data['list'] = [m.to_dict() for m in mods] + ret['data'] = data except Exception as e: ret['code'] = 400 ret['message'] = str(e) - print('Server get all models done.') + print(f'Server get all models done. {ret}') return jsonify(ret) def modify_model(self): @@ -193,11 +199,12 @@ class GazeboSimHttpServer(HttpRPCServer): try: # 解析参数 info = request.get_json() - scene_id = info.get('scene_id', type=int, default=None) + print(f'get mod req {info}') + scene_id = info.get('id') 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) + object_id = info.get('object_id') if not object_id: raise ValueError('Object ID is required') # 制作Model @@ -219,7 +226,8 @@ class GazeboSimHttpServer(HttpRPCServer): try: # 解析参数 info = request.get_json() - scene_id = info.get('scene_id', type=int, default=None) + print(f'get add req {info}') + scene_id = info.get('id') 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}') @@ -241,11 +249,12 @@ class GazeboSimHttpServer(HttpRPCServer): try: # 解析参数 info = request.get_json() - scene_id = info.get('scene_id', type=int, default=None) + print(f'get del req {info}') + scene_id = info.get('id') 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) + object_id = info.get('object_id') # 删除模型 self.model_mgr.remove_model(object_id) except Exception as e: diff --git a/model_mgr/manager.py b/model_mgr/manager.py index a864533..8fed963 100644 --- a/model_mgr/manager.py +++ b/model_mgr/manager.py @@ -37,21 +37,25 @@ class ModelManager: if not add_sim: return # TODO: (谁爱做谁做)场景决定id-类型定义 + if model.obj_type == ModelType.ROBOT: + raise ValueError(f'Cannot add robot model {model.id} {model.name}') if model.obj_type == ModelType.TABLE: self.generator.generate_table(name=model.name, x=model.pose.position.x, y=model.pose.position.y, height=model.pose.position.z, roll=model.pose.orientation.roll, pitch=model.pose.orientation.pitch, yaw=model.pose.orientation.yaw) elif model.obj_type == ModelType.TAG: - self.generator.generate_apriltag(name=model.name, tag_id=model.id, x=model.pose.position.x, y=model.pose.position.y, + self.generator.generate_apriltag(name=model.name, tag_id=model.tag_id, x=model.pose.position.x, y=model.pose.position.y, z=model.pose.position.z, roll=model.pose.orientation.roll, pitch=model.pose.orientation.pitch, yaw=model.pose.orientation.yaw) elif model.obj_type == ModelType.BOX: self.generator.generate_box_with_apriltag(name=model.name, l=model.size.length, w=model.size.width, h=model.size.height, - tag_id=model.id, x=model.pose.position.x, y=model.pose.position.y, z=model.pose.position.z, + tag_id=model.tag_id, x=model.pose.position.x, y=model.pose.position.y, z=model.pose.position.z, roll=model.pose.orientation.roll, pitch=model.pose.orientation.pitch, yaw=model.pose.orientation.yaw) def remove_model(self, model_id: int): with self.rlock: # 信息删除 model = self.model_id_dict.get(model_id) + if model.obj_type == ModelType.ROBOT: + raise ValueError(f'Cannot remove robot model {model_id} {model.name}') name = model.name del self.model_id_dict[model_id] del self.model_name_dict[name] @@ -76,6 +80,8 @@ class ModelManager: def modify_model(self, model: Model): 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}') self.remove_model(model.id) self.add_model(model) diff --git a/model_mgr/model_types.py b/model_mgr/model_types.py index b77a668..8025485 100644 --- a/model_mgr/model_types.py +++ b/model_mgr/model_types.py @@ -150,7 +150,7 @@ class Model: "obj_type": int(self.obj_type), "id": self.id, "pose": self.pose.to_dict(), - "description": self.description, + "describe": self.description, # 输出为dict/json时名为describe "ability_code": self.ability_code, "tag_id": self.tag_id, "mass": self.mass, diff --git a/scene/grasp_box/config.json b/scene/grasp_box/config.json index 47ddac1..ce05973 100644 --- a/scene/grasp_box/config.json +++ b/scene/grasp_box/config.json @@ -15,7 +15,7 @@ "description": "a table model", "pose": { "position": { - "x": 1, + "x": 0, "y": 2, "z": 0.8 }, @@ -34,7 +34,7 @@ "description": "a table model", "pose": { "position": { - "x": 1, + "x": 0, "y": -2, "z": 0.8 }, @@ -53,7 +53,7 @@ "description": "a box model", "pose": { "position": { - "x": 1, + "x": 0, "y": 2, "z": 1.0 }, @@ -79,7 +79,7 @@ "description": "a tag model", "pose": { "position": { - "x": 1, + "x": 0, "y": -1.8, "z": 1.6 }, diff --git a/scene/grasp_box/pids.txt b/scene/grasp_box/pids.txt deleted file mode 100644 index 3d4e63e..0000000 --- a/scene/grasp_box/pids.txt +++ /dev/null @@ -1,3 +0,0 @@ -513161 -513368 -514751 diff --git a/scene_mgr/manager.py b/scene_mgr/manager.py index 22f892d..dd47f84 100644 --- a/scene_mgr/manager.py +++ b/scene_mgr/manager.py @@ -1,10 +1,8 @@ -import dataclasses -import importlib import json import os import signal import subprocess -import threading +import time from .scene_types import PreloadModel, Scene @@ -66,7 +64,7 @@ class SceneManager: if name not in self.scene_name_dict: raise ValueError(f'scene {name} not found') self.pid_file = os.path.join( - os.path.dirname(__file__), 'scene', name, 'pids.txt') + 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(