zpf/gazebo_world_manager
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 添加模型管理层

Browse Source
This commit is contained in:
zpff 2025-08-11 20:03:19 +08:00
parent 26503a1a6a
commit 35a87ce471
13 changed files with 604 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gazebo_ctrl.py
View File

@ -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)
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)

182
http_server.py
View File

@ -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('/<scene>/model/state',
self.get_scene_model_state, ['GET'])
self.add_route('/<scene>/model/state',
self.set_scene_model_state, ['POST'])
self.add_route('/<scene>/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()

64
model_mgr/generator.py Normal file
View File

@ -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)

116
model_mgr/manager.py Normal file
View File

@ -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())

153
model_mgr/model_types.py Normal file
View File

@ -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())

95
scene/grasp_box/config.json Normal file
View File

@ -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
}
]
}

86
scene/grasp_box/gen_models.py
View File

@ -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)

3
scene/grasp_box/pids.txt Normal file
View File

@ -0,0 +1,3 @@
513161
513368
514751

149
scene/grasp_box/plugin.py
View File

@ -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

66
scene_mgr.py → scene_mgr/manager.py
View File

@ -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')

49
scene_mgr/scene_types.py Normal file
View File

@ -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,
}

16
scene_plugin.py
View File

@ -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()

25
test.py Normal file
View File

@ -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)