From 35a87ce4715123830f9a84a893263270148def88 Mon Sep 17 00:00:00 2001 From: zpff Date: Mon, 11 Aug 2025 20:03:19 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E7=AE=A1=E7=90=86=E5=B1=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gazebo_ctrl.py | 23 +++- http_server.py | 182 ++++++--------------------- model_mgr/generator.py | 64 ++++++++++ model_mgr/manager.py | 116 +++++++++++++++++ model_mgr/model_types.py | 153 ++++++++++++++++++++++ scene/grasp_box/config.json | 95 ++++++++++++++ scene/grasp_box/gen_models.py | 86 ------------- scene/grasp_box/pids.txt | 3 + scene/grasp_box/plugin.py | 149 ---------------------- scene_mgr.py => scene_mgr/manager.py | 66 ++++++---- scene_mgr/scene_types.py | 49 ++++++++ scene_plugin.py | 16 --- test.py | 25 ++++ 13 files changed, 604 insertions(+), 423 deletions(-) create mode 100644 model_mgr/generator.py create mode 100644 model_mgr/manager.py create mode 100644 model_mgr/model_types.py create mode 100644 scene/grasp_box/config.json delete mode 100644 scene/grasp_box/gen_models.py create mode 100644 scene/grasp_box/pids.txt delete mode 100644 scene/grasp_box/plugin.py rename scene_mgr.py => scene_mgr/manager.py (62%) create mode 100644 scene_mgr/scene_types.py delete mode 100644 scene_plugin.py create mode 100644 test.py diff --git a/gazebo_ctrl.py b/gazebo_ctrl.py index d7c0d13..3e8a02a 100644 --- a/gazebo_ctrl.py +++ b/gazebo_ctrl.py @@ -5,7 +5,7 @@ from gazebo_msgs.msg import ModelState from gazebo_msgs.srv import (DeleteModel, GetLinkProperties, GetModelProperties, GetModelPropertiesResponse, GetModelState, GetModelStateResponse, - GetWorldProperties, SetModelState) + GetWorldProperties, SetModelState, SpawnModel) from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 from std_srvs.srv import Empty from tf.transformations import euler_from_quaternion, quaternion_from_euler @@ -229,9 +229,26 @@ class GazeboROSController: raise RuntimeError( f"Failed to set model state for {name}: {resp.status_message}") + def spawn_urdf_model(self, model_name, urdf_content, pose): + # 生成模型 + rospy.wait_for_service('/gazebo/spawn_urdf_model') + spawn_service = rospy.ServiceProxy( + '/gazebo/spawn_urdf_model', SpawnModel) + response = spawn_service( + model_name=model_name, + model_xml=urdf_content, + robot_namespace='', + initial_pose=pose, + reference_frame='world' + ) + if not response.success: + raise RuntimeError( + f"Failed to spawn {model_name}: {response.status_message}") + if __name__ == '__main__': controller = GazeboROSController() print(json.dumps(controller.get_model_state_f(['block']))) - jsond = {"pose": {"position": {"x": 1, "y": 2.0000041832461184, "z": 1}, "orientation": {"roll": 1.0008074575547628e-10, "pitch": -8.228587019137734e-12, "yaw": -3.1415190381173597}}, "twist": {"linear": {"x": 1.6640674574453256e-11, "y": 2.6277239545161537e-10, "z": 1.607170552758444e-05}, "angular": {"x": -2.2534679599858193e-09, "y": 1.551234375963502e-10, "z": -2.5445035969479123e-13}}, "name": "block"} - controller.set_model_state('block', jsond) \ No newline at end of file + jsond = {"pose": {"position": {"x": 1, "y": 2.0000041832461184, "z": 1}, "orientation": {"roll": 1.0008074575547628e-10, "pitch": -8.228587019137734e-12, "yaw": -3.1415190381173597}}, "twist": {"linear": { + "x": 1.6640674574453256e-11, "y": 2.6277239545161537e-10, "z": 1.607170552758444e-05}, "angular": {"x": -2.2534679599858193e-09, "y": 1.551234375963502e-10, "z": -2.5445035969479123e-13}}, "name": "block"} + controller.set_model_state('block', jsond) diff --git a/http_server.py b/http_server.py index 718f744..fb5df56 100644 --- a/http_server.py +++ b/http_server.py @@ -1,6 +1,4 @@ import dataclasses -import importlib -import os import socket import subprocess import threading @@ -12,7 +10,8 @@ import werkzeug.serving from flask import jsonify, request import gazebo_ctrl -import scene_mgr +from model_mgr.manager import ModelManager +from scene_mgr.manager import SceneManager @dataclasses.dataclass @@ -83,158 +82,57 @@ class GazeboSimHttpServer(HttpRPCServer): def __init__(self): super().__init__() - self.scene_mgr = scene_mgr.SceneManager() + self.rlock = threading.RLock() + self.scene_mgr = SceneManager() + self.model_mgr = ModelManager() # 启动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']) - self.add_route('/simulation/start', self.start_simulation, ['GET']) - self.add_route('/simulation/shutdown', - self.shutdown_simulation, ['GET']) - self.add_route('/simulation/run-script', self.run_script, ['POST']) + # 添加接口 + 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']) - # Gazebo仿真/仿真控制 - self.add_route('/physics/continue', self.continue_physics, ['GET']) - self.add_route('/physics/pause', self.pause_physics, ['GET']) - self.add_route('/physics/reset', self.reset_physics, ['GET']) + def query_scenes(self): + # 查询场景 + with self.rlock: + pass - # Gazebo仿真/模型控制 - self.add_route('/model/state', self.get_model_state, ['GET']) - self.add_route('/model/state', self.set_model_state, ['POST']) - self.add_route('/model/spawn', self.spawn_model, ['POST']) - self.add_route('/model/delete', self.delete_model, ['POST']) + def start_scene(self): + # 启动场景 + with self.rlock: + pass - # Gazebo仿真/场景专有控制 - self.add_route('//model/state', - self.get_scene_model_state, ['GET']) - self.add_route('//model/state', - self.set_scene_model_state, ['POST']) - self.add_route('//model/spawn', - self.spawn_scene_model, ['POST']) + def shut_scene(self): + # 关闭场景，仅供start_scene、心跳线程使用 + with self.rlock: + pass - # ===== 处理函数实现 ===== - 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 get_all_models(self): + # 获取所有模型 + with self.rlock: + pass - 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 + def modify_model(self): + # 修改模型 + with self.rlock: + pass - def shutdown_simulation(self): - try: - if not self.scene_name: - return 'No scene is running', 400 - # TODO: 启动脚本创建了很多进程，通过关-开-关使它们因冲突而结束，获得干净环境 - def my_shut(): - self.scene_mgr.shut_scene() - time.sleep(2) - self.scene_mgr.start_scene(self.scene_name) - time.sleep(15) - self.scene_mgr.shut_scene() - self.gazebo_ctrl = None - self.scene_name = None - threading.Thread(target=my_shut).start() - except Exception as e: - return str(e), 400 - return 'OK', 200 - - def run_script(self): - name = request.args.get('name') - try: - self.scene_mgr.run_script(name) - except Exception as e: - return str(e), 400 - return 'OK', 200 - - def continue_physics(self): - try: - self.gazebo_ctrl.continue_physics() - except Exception as e: - return str(e), 400 - return 'OK', 200 - - def pause_physics(self): - try: - self.gazebo_ctrl.pause_physics() - except Exception as e: - return str(e), 400 - return 'OK', 200 - - def reset_physics(self): - 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 [] - return self.gazebo_ctrl.get_model_state_f(names) - - def set_model_state(self): - # TODO: - data = request.get_json(silent=True) - if not data or 'name' not in data: - return 'Missing required field name', 400 - return 'model state not updated', 200 - - def spawn_model(self): - # TODO: - data = request.get_json(silent=True) - required = ['pose', 'name', 'xml'] - if not data or not all(k in data for k in required): - 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 '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 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 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): - # 横杠转下划线 - scene = scene.replace('-', '_') - if scene != self.scene_name: - return f'running {self.scene_name} but try to access {scene}', 400 - return self.scene_mgr.scene_plugin.post_scene_model_spawn() + def add_model(self): + # 添加模型 + with self.rlock: + pass + def remove_model(self): + # 删除模型 + with self.rlock: + pass if __name__ == '__main__': server = GazeboSimHttpServer() diff --git a/model_mgr/generator.py b/model_mgr/generator.py new file mode 100644 index 0000000..a590d4a --- /dev/null +++ b/model_mgr/generator.py @@ -0,0 +1,64 @@ +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 + +from gazebo_ctrl import GazeboROSController + + +class ModelGenerator: + def __init__(self): + self.controller = GazeboROSController() + ros_package_path = os.environ.get('ROS_PACKAGE_PATH', '') + self.base_dir = os.path.expanduser('~') + self.base_path = os.path.join(self.base_dir, 'gazebo_world_manager') + if self.base_path not in ros_package_path.split(':'): + os.environ['ROS_PACKAGE_PATH'] = f"{self.base_path}:{ros_package_path}" if ros_package_path else self.base_path + + def generate_table(self, name, x=0, y=0, height=1, roll=0, pitch=0, yaw=0): + xacro_file = os.path.join(self.base_path, "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=height), + orientation=Quaternion(*quaternion_from_euler(roll, pitch, yaw)) + ) + return self.controller.spawn_urdf_model(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 = os.path.join(self.base_path, "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)) + ) + return self.controller.spawn_urdf_model(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 = os.path.join(self.base_path, "scene/grasp_box/models/box.xacro") + urdf_content = subprocess.check_output([ + 'rosrun', 'xacro', 'xacro', xacro_file, f'box_length:={l}', f'box_width:={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)) + ) + return self.controller.spawn_urdf_model(name, urdf_content, pose) + + +if __name__ == '__main__': + generator = ModelGenerator() + # 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) diff --git a/model_mgr/manager.py b/model_mgr/manager.py new file mode 100644 index 0000000..a864533 --- /dev/null +++ b/model_mgr/manager.py @@ -0,0 +1,116 @@ +import threading +from typing import List + +from gazebo_ctrl import GazeboROSController +from model_mgr.generator import ModelGenerator +from model_mgr.model_types import (Model, ModelType, Orientation, Pose, + Position, Size) +from scene_mgr.scene_types import PreloadModel + + +class ModelManager: + def __init__(self): + self.controller = GazeboROSController() + self.generator = ModelGenerator() + self.model_name_dict = {} + self.model_id_dict = {} + self.rlock = threading.RLock() + + def load_models(self, pmodels: List[PreloadModel]): + with self.rlock: + for pmod in pmodels: + self.add_model(pmod.model, pmod.gen) + + def add_model(self, model: Model, add_sim: bool = True): + with self.rlock: + if self.model_name_dict.get(model.name): + raise ValueError( + f"Model with name {model.name} already exists.") + if self.model_id_dict.get(model.id): + raise ValueError(f"Model with id {model.id} already exists.") + if model.obj_type == ModelType.UNKNOWN or model.obj_type not in ModelType: + raise ValueError(f"Invalid model type: {model.obj_type}") + # 信息添加 + self.model_name_dict[model.name] = model + self.model_id_dict[model.id] = model + # 仿真添加 + if not add_sim: + return + # TODO: （谁爱做谁做）场景决定id-类型定义 + 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, + 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, + 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) + name = model.name + del self.model_id_dict[model_id] + del self.model_name_dict[name] + # 仿真删除 + self.controller.delete_model(name) + + def get_model(self, model_id: int) -> Model: + with self.rlock: + mod = self.model_id_dict.get(model_id, None) + if not mod: + raise ValueError(f"Model with id {model_id} does not exist.") + self._update_model_pose_from_gazebo(mod) + return mod + + def get_all_models(self) -> List[Model]: + with self.rlock: + li = list(self.model_id_dict.values()) + for mod in li: + self._update_model_pose_from_gazebo(mod) + return li + + def modify_model(self, model: Model): + with self.rlock: + # 视觉元素tag.id、box.size、table.height无法动态修改，于是我们选择先删除再添加 + self.remove_model(model.id) + self.add_model(model) + + def _update_model_pose_from_gazebo(self, model: Model): + with self.rlock: + name = model.name + info = self.controller.get_model_state_f([name]) + if info: + model.pose = Pose.from_dict(info[0]['pose']) + + def _update_model_dict(self, model): + with self.rlock: + self.model_id_dict[model.id] = model + self.model_name_dict[model.name] = model + + +if __name__ == "__main__": + mgr = ModelManager() + mod = Model( + id=1, + name="box", + obj_type=ModelType.BOX, + pose=Pose( + position=Position(x=2, y=1, z=1), + orientation=Orientation(roll=0, pitch=0, yaw=0) + ), + size=Size(length=0.5, width=0.5, height=0.5) + ) + mgr.add_model(mod) + liss = mgr.get_all_models() + for m in liss: + print(m.to_dict()) + mod.pose.position.x = 3 + mod.pose.position.y = 2 + mgr.modify_model(mod) + print(mgr.get_model(1).to_dict()) + mgr.remove_model(1) + print(mgr.get_all_models()) diff --git a/model_mgr/model_types.py b/model_mgr/model_types.py new file mode 100644 index 0000000..31e8970 --- /dev/null +++ b/model_mgr/model_types.py @@ -0,0 +1,153 @@ +import dataclasses +import enum +import threading + +from tf.transformations import euler_from_quaternion, quaternion_from_euler + + +@dataclasses.dataclass +class Position: + x: float = 0 + y: float = 0 + z: float = 0 + + def to_dict(self): + return {"x": self.x, "y": self.y, "z": self.z} + + @classmethod + def from_dict(cls, data): + return cls( + x=data.get("x", 0), + y=data.get("y", 0), + z=data.get("z", 0), + ) + + +@dataclasses.dataclass +class Orientation: + roll: float = 0 + pitch: float = 0 + yaw: float = 0 + + def to_dict(self): + return {"roll": self.roll, "pitch": self.pitch, "yaw": self.yaw} + + @classmethod + def from_dict(cls, data): + return cls( + roll=data.get("roll", 0), + pitch=data.get("pitch", 0), + yaw=data.get("yaw", 0), + ) + + def to_quaternion(self): + return quaternion_from_euler(self.roll, self.pitch, self.yaw, 'rxyz') + + def from_quaternion(self, w, x, y, z): + self.roll, self.pitch, self.yaw = euler_from_quaternion(w, x, y, z) + + +@dataclasses.dataclass +class Pose: + position: Position = dataclasses.field(default_factory=Position) + orientation: Orientation = dataclasses.field(default_factory=Orientation) + + def to_dict(self): + return { + "position": self.position.to_dict(), + "orientation": self.orientation.to_dict(), + } + + @classmethod + def from_dict(cls, data): + return cls( + position=Position.from_dict(data.get("position", {})), + orientation=Orientation.from_dict(data.get("orientation", {})), + ) + + +@dataclasses.dataclass +class Size: + length: float = 0 + width: float = 0 + height: float = 0 + + def to_dict(self): + return { + "length": self.length, + "width": self.width, + "height": self.height, + } + + @classmethod + def from_dict(cls, data): + return cls( + length=data.get("length", 0), + width=data.get("width", 0), + height=data.get("height", 0), + ) + + +_nr_model_id = 0 +_nr_model_id_lock = threading.Lock() + + +class ModelType(enum.IntEnum): + UNKNOWN = 0 + ROBOT = 1 + BOX = 2 + TABLE = 3 + TAG = 4 + + +@dataclasses.dataclass +class Model: + name: str + obj_type: ModelType = ModelType.UNKNOWN + id: int = -1 + pose: Pose = dataclasses.field(default_factory=Pose) + description: str = "" # 输出为dict/json时名为describe + # 以下为选填 + ability_code: list = dataclasses.field(default_factory=list) + tag_id: int = 0 + mass: int = 1 + size: Size = dataclasses.field(default_factory=Size) + + def __post_init__(self): + global _nr_model_id + if self.id == -1: + with _nr_model_id_lock: + self.id = _nr_model_id + _nr_model_id += 1 + + def to_dict(self): + return { + "name": self.name, + "obj_type": int(self.obj_type), + "id": self.id, + "pose": self.pose.to_dict(), + "description": self.description, + "ability_code": self.ability_code, + "tag_id": self.tag_id, + "mass": self.mass, + "size": self.size.to_dict(), + } + + @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), + pose=Pose.from_dict(data.get("pose", {})), + description=data.get("description", ""), + ability_code=data.get("ability_code", []), + tag_id=data.get("tag_id", 0), + mass=data.get("mass", 1), + size=Size.from_dict(data.get("size", {})), + ) + + +if __name__ == "__main__": + a = Model(name='123') + print(a.to_dict()) diff --git a/scene/grasp_box/config.json b/scene/grasp_box/config.json new file mode 100644 index 0000000..47ddac1 --- /dev/null +++ b/scene/grasp_box/config.json @@ -0,0 +1,95 @@ +{ + "preload_models": [ + { + "gen": false, + "id": 1, + "name": "biped_s42", + "obj_type": 1, + "description": "a biped robot model" + }, + { + "gen": true, + "id": 2, + "name": "table1", + "obj_type": 3, + "description": "a table model", + "pose": { + "position": { + "x": 1, + "y": 2, + "z": 0.8 + }, + "orientation": { + "row": 0, + "pitch": 0, + "yaw": 0 + } + } + }, + { + "gen": true, + "id": 3, + "name": "table2", + "obj_type": 3, + "description": "a table model", + "pose": { + "position": { + "x": 1, + "y": -2, + "z": 0.8 + }, + "orientation": { + "row": 0, + "pitch": 0, + "yaw": 0 + } + } + }, + { + "gen": true, + "id": 4, + "name": "box", + "obj_type": 2, + "description": "a box model", + "pose": { + "position": { + "x": 1, + "y": 2, + "z": 1.0 + }, + "orientation": { + "row": 0, + "pitch": 0, + "yaw": 0 + } + }, + "mass": 1, + "size": { + "length": 0.25, + "width": 0.2, + "height": 0.2 + }, + "tag_id": 1 + }, + { + "gen": true, + "id": 5, + "name": "tag", + "obj_type": 4, + "description": "a tag model", + "pose": { + "position": { + "x": 1, + "y": -1.8, + "z": 1.6 + }, + "orientation": { + "row": 0, + "pitch": 1.57, + "yaw": 1.57 + } + }, + "tag_id": 0 + } + ] +} \ No newline at end of file diff --git a/scene/grasp_box/gen_models.py b/scene/grasp_box/gen_models.py deleted file mode 100644 index eeebb54..0000000 --- a/scene/grasp_box/gen_models.py +++ /dev/null @@ -1,86 +0,0 @@ -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): - 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', '') - 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}") - return False, f"Failed to spawn {model_name}: {response.status_message}" - 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, 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=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" - 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)) - ) - return 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_width:={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)) - ) - return self._generate(name, urdf_content, pose) - - -if __name__ == '__main__': - generator = GraspModelGenerator() - # 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) diff --git a/scene/grasp_box/pids.txt b/scene/grasp_box/pids.txt new file mode 100644 index 0000000..3d4e63e --- /dev/null +++ b/scene/grasp_box/pids.txt @@ -0,0 +1,3 @@ +513161 +513368 +514751 diff --git a/scene/grasp_box/plugin.py b/scene/grasp_box/plugin.py deleted file mode 100644 index 528d535..0000000 --- a/scene/grasp_box/plugin.py +++ /dev/null @@ -1,149 +0,0 @@ -import dataclasses -import time - -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): - 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: - # 视觉元素tag.id、box.size、table.height无法动态修改，于是我们选择先删除再添加 - data = request.get_json(force=True) - name = data.get('name') - self.gz_ctrl.delete_model(name) - time.sleep(0.01) - return self.post_scene_model_spawn() - except Exception as e: - return str(e), 400 - - -if __name__ == '__main__': - pass diff --git a/scene_mgr.py b/scene_mgr/manager.py similarity index 62% rename from scene_mgr.py rename to scene_mgr/manager.py index 189b2f9..9a4736d 100644 --- a/scene_mgr.py +++ b/scene_mgr/manager.py @@ -1,23 +1,18 @@ import dataclasses import importlib +import json import os import signal import subprocess import threading -import scene_plugin - - -@dataclasses.dataclass -class Scene: - name: str - launch_file: str = None - scripts: dict = None +from .scene_types import PreloadModel, Scene class SceneManager: def __init__(self): - self.scene_dict = {} + self.scene_name_dict = {} + self.scene_id_dict = {} self.running_scene = None self.sim_process = None self.sim_thread = None @@ -28,14 +23,16 @@ class SceneManager: """ 从场景文件夹加载场景。 """ - scene_dir = os.path.join(os.path.dirname(__file__), 'scene') + scene_dir = os.path.join(os.path.dirname(__file__), '..', 'scene') if not os.path.exists(scene_dir): raise RuntimeError('no scene exist') names = [name for name in os.listdir( scene_dir) if os.path.isdir(os.path.join(scene_dir, name))] for name in names: scene = Scene(name) + # 启动文件 scene.launch_file = os.path.join(scene_dir, name, 'launch.sh') + # 脚本 scene.scripts = {} if not os.path.exists(scene.launch_file): continue @@ -44,32 +41,40 @@ class SceneManager: script_name_only = os.path.splitext(script_name)[0] scene.scripts[script_name_only] = os.path.join( scene_dir, name, 'scripts', script_name) - self.scene_dict[name] = scene + # 配置文件 + config_file = os.path.join(scene_dir, name, 'config.json') + # 需要预加载的模型 + with open(config_file, 'r') as f: + config = json.load(f) + for model_data in config.get('preload_models', []): + pmodel = PreloadModel.from_dict(model_data) + scene.preload_models.append(pmodel) + self.scene_name_dict[name] = scene + self.scene_id_dict[scene.id] = scene def get_scene(self): - return list(self.scene_dict.keys()) + return list(self.scene_name_dict.values()) def get_scripts(self, name): - return list(self.scene_dict[name].scripts.keys()) + return list(self.scene_name_dict[name].scripts.keys()) - def start_scene(self, name): + def start_scene(self, id): + name = self.scene_id_dict[id].name if self.running_scene: raise RuntimeError( f'scene {self.running_scene} is already running') - if name not in self.scene_dict: + 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') 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], + ['bash', self.scene_name_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.running_scene = id def shut_scene(self): with open(self.pid_file, 'r') as f: @@ -84,20 +89,27 @@ class SceneManager: print(f"invalid PID: {pid}") os.remove(self.pid_file) self.pid_file = None - print(f'killing launch PID: {self.sim_process.pid}') - self.sim_process.kill() - # 杀死默认配置文件启动的gazebo和rviz - subprocess.Popen(['pkill', '-f', 'gzserver']) - subprocess.Popen(['pkill', '-f', 'gzclient']) - subprocess.Popen(['pkill', '-f', 'rviz']) + 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.running_scene = None - self.scene_plugin = 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') - script_path = self.scene_dict[self.running_scene].scripts.get( + script_path = self.scene_id_dict[self.running_scene].scripts.get( name, None) if not script_path: raise ValueError(f'script {name} not found') diff --git a/scene_mgr/scene_types.py b/scene_mgr/scene_types.py new file mode 100644 index 0000000..aa54e2a --- /dev/null +++ b/scene_mgr/scene_types.py @@ -0,0 +1,49 @@ +import dataclasses +import threading +from typing import List + +from model_mgr.model_types import Model + + +@dataclasses.dataclass +class PreloadModel: + gen: bool = False + model: Model = dataclasses.field(default_factory=Model) + + @classmethod + def from_dict(cls, data): + return cls( + gen=data.get('gen', False), + model=Model.from_dict(data) + ) + + +_nr_scene_id = 0 +_nr_scene_id_lock = threading.Lock() + + +@dataclasses.dataclass +class Scene: + name: str + id: int = -1 + version: str = '0.1.0' + description: str = 'a scene' + size: str = '260k' + launch_file: str = None + preload_models: List[PreloadModel] = dataclasses.field(default_factory=list) + + def __post_init__(self): + global _nr_scene_id + if self.id == -1: + with _nr_scene_id_lock: + _nr_scene_id += 1 + self.id = _nr_scene_id + + def to_dict(self): + return { + 'name': self.name, + 'id': self.id, + 'version': self.version, + 'description': self.description, + 'size': self.size, + } diff --git a/scene_plugin.py b/scene_plugin.py deleted file mode 100644 index c97ff57..0000000 --- a/scene_plugin.py +++ /dev/null @@ -1,16 +0,0 @@ -class ScenePluginBase: - """ - 场景自定义模型的处理。 - """ - - 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() diff --git a/test.py b/test.py new file mode 100644 index 0000000..f43e674 --- /dev/null +++ b/test.py @@ -0,0 +1,25 @@ +import dataclasses +import socket +import subprocess +import threading +import time + +import flask +import rospy +import werkzeug.serving +from flask import jsonify, request + +import gazebo_ctrl +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)