feat: 实现一般模型修改，诡异程序停止、场景模型修改
This commit is contained in:
zpff
parent
c9a819be35
commit
26503a1a6a
29
1.txt
29
1.txt
@ -1,29 +0,0 @@
|
||||
curl --location --request GET 'http://127.0.0.1:12300/simulation/start?scene=grasp-box'
|
||||
|
||||
curl --location --request GET 'http://127.0.0.1:12300/simulation/shutdown'
|
||||
|
||||
curl --location --request POST 'http://127.0.0.1:12300/grasp-box/model/spawn' \
|
||||
--header 'Content-Type: application/json' \
|
||||
--data-raw '{
|
||||
"name": "box10",
|
||||
"type": "box",
|
||||
"pose": {
|
||||
"position": {
|
||||
"x": 2,
|
||||
"y": 2,
|
||||
"z": 1
|
||||
},
|
||||
"orientation": {
|
||||
"roll": 0,
|
||||
"pitch": 0,
|
||||
"yaw": 0
|
||||
}
|
||||
},
|
||||
"tag_id": 9,
|
||||
"mass": 1,
|
||||
"size": {
|
||||
"l": 0.3,
|
||||
"h": 0.2,
|
||||
"w": 0.2
|
||||
}
|
||||
}'
|
||||
@ -1,8 +1,12 @@
|
||||
import json
|
||||
|
||||
import rospy
|
||||
from gazebo_msgs.msg import ModelState
|
||||
from gazebo_msgs.srv import (DeleteModel, GetLinkProperties,
|
||||
GetModelProperties, GetModelPropertiesResponse,
|
||||
GetModelState, GetModelStateResponse,
|
||||
GetWorldProperties)
|
||||
GetWorldProperties, SetModelState)
|
||||
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
|
||||
|
||||
@ -10,7 +14,8 @@ from tf.transformations import euler_from_quaternion, quaternion_from_euler
|
||||
class GazeboROSController:
|
||||
def __init__(self):
|
||||
if not rospy.core.is_initialized():
|
||||
rospy.init_node('gazebo_world_manager', anonymous=True, disable_signals=True)
|
||||
rospy.init_node('gazebo_world_manager',
|
||||
anonymous=True, disable_signals=True)
|
||||
self.model_info = None
|
||||
|
||||
def get_model_names(self) -> list:
|
||||
@ -171,10 +176,62 @@ class GazeboROSController:
|
||||
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
|
||||
resp = delete_model(model_name=name)
|
||||
if not resp.success:
|
||||
raise RuntimeError(f"Failed to delete model {name}: {resp.status_message}")
|
||||
raise RuntimeError(
|
||||
f"Failed to delete model {name}: {resp.status_message}")
|
||||
|
||||
def set_model_state(self, name, state_json):
|
||||
# 解析位置
|
||||
pos = state_json.get('pose', {}).get('position', {})
|
||||
x = pos.get('x', 0.0)
|
||||
y = pos.get('y', 0.0)
|
||||
z = pos.get('z', 0.0)
|
||||
|
||||
# 解析欧拉角并转为四元数
|
||||
ori = state_json.get('pose', {}).get('orientation', {})
|
||||
roll = ori.get('roll', 0.0)
|
||||
pitch = ori.get('pitch', 0.0)
|
||||
yaw = ori.get('yaw', 0.0)
|
||||
qx, qy, qz, qw = quaternion_from_euler(roll, pitch, yaw)
|
||||
|
||||
# 解析twist
|
||||
linear = state_json.get('twist', {}).get('linear', {})
|
||||
angular = state_json.get('twist', {}).get('angular', {})
|
||||
linear_vec = Vector3(
|
||||
x=linear.get('x', 0.0),
|
||||
y=linear.get('y', 0.0),
|
||||
z=linear.get('z', 0.0)
|
||||
)
|
||||
angular_vec = Vector3(
|
||||
x=angular.get('x', 0.0),
|
||||
y=angular.get('y', 0.0),
|
||||
z=angular.get('z', 0.0)
|
||||
)
|
||||
# 参考系
|
||||
reference_frame = state_json.get('reference', 'world')
|
||||
|
||||
# 构造ModelState
|
||||
state = ModelState()
|
||||
state.model_name = name
|
||||
state.pose = Pose(
|
||||
position=Point(x, y, z),
|
||||
orientation=Quaternion(qx, qy, qz, qw)
|
||||
)
|
||||
state.twist = Twist(
|
||||
linear=linear_vec,
|
||||
angular=angular_vec
|
||||
)
|
||||
state.reference_frame = reference_frame
|
||||
rospy.wait_for_service('/gazebo/set_model_state')
|
||||
set_model_state = rospy.ServiceProxy(
|
||||
'/gazebo/set_model_state', SetModelState)
|
||||
resp = set_model_state(state)
|
||||
if not resp.success:
|
||||
raise RuntimeError(
|
||||
f"Failed to set model state for {name}: {resp.status_message}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
controller = GazeboROSController()
|
||||
import json
|
||||
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)
|
||||
@ -140,9 +140,18 @@ class GazeboSimHttpServer(HttpRPCServer):
|
||||
|
||||
def shutdown_simulation(self):
|
||||
try:
|
||||
self.scene_mgr.shut_scene()
|
||||
self.gazebo_ctrl = None
|
||||
self.scene_name = None
|
||||
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
|
||||
@ -178,15 +187,17 @@ class GazeboSimHttpServer(HttpRPCServer):
|
||||
|
||||
def get_model_state(self):
|
||||
names = request.args.getlist('names') or []
|
||||
return jsonify([{'name': n, 'pose': {}, 'twist': {}} for n in names])
|
||||
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 jsonify({'error': 'Missing required field name'}), 400
|
||||
return jsonify({'status': 'model state updated', 'data': 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):
|
||||
@ -218,7 +229,6 @@ class GazeboSimHttpServer(HttpRPCServer):
|
||||
return self.scene_mgr.scene_plugin.post_scene_model_state()
|
||||
|
||||
def spawn_scene_model(self, scene):
|
||||
print(111)
|
||||
# 横杠转下划线
|
||||
scene = scene.replace('-', '_')
|
||||
if scene != self.scene_name:
|
||||
|
||||
@ -67,10 +67,9 @@ class GraspModelGenerator:
|
||||
|
||||
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的盒子模型"""
|
||||
print(f'{name}:x={x},y={y},z={z}')
|
||||
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_weight:={w}', f'box_height:={h}', f'box_mass:={mass}', f'tag_id:={tag_id}'
|
||||
'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),
|
||||
|
||||
@ -26,8 +26,6 @@ trap 'echo -e "\033[31m接收到 Ctrl+C，清理后台进程...\033[0m"; \
|
||||
while read -r pid; do \
|
||||
kill $pid 2>/dev/null && echo -e "\033[32m已终止进程 PID: $pid\033[0m"; \
|
||||
done < "$PID_FILE"; \
|
||||
rm "$PID_FILE"; \
|
||||
echo -e "\033[32m已删除 PID 文件: $PID_FILE\033[0m"; \
|
||||
fi; \
|
||||
exit 0' SIGINT
|
||||
|
||||
@ -75,8 +73,5 @@ else
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# # 清理后台进程
|
||||
# echo -e "\033[32m清理后台 ROS 进程...\033[0m"
|
||||
# kill $WEB_PID $GAZEBO_PID $STRATEGIES_PID 2>/dev/null
|
||||
# rm "$PID_FILE" 2>/dev/null
|
||||
# echo -e "\033[32m已删除 PID 文件: $PID_FILE\033[0m"
|
||||
# 在这里阻塞
|
||||
wait
|
||||
@ -1,3 +0,0 @@
|
||||
23376
|
||||
23579
|
||||
24764
|
||||
@ -1,4 +1,5 @@
|
||||
import dataclasses
|
||||
import time
|
||||
|
||||
from flask import jsonify, request
|
||||
|
||||
@ -38,7 +39,6 @@ class Plugin(scene_plugin.ScenePluginBase):
|
||||
self.gz_ctrl = gazebo_ctrl.GazeboROSController()
|
||||
|
||||
def post_scene_model_spawn(self):
|
||||
print(222)
|
||||
try:
|
||||
# 解析参数
|
||||
data = request.get_json(force=True)
|
||||
@ -123,7 +123,8 @@ class Plugin(scene_plugin.ScenePluginBase):
|
||||
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}
|
||||
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':
|
||||
@ -131,45 +132,18 @@ class Plugin(scene_plugin.ScenePluginBase):
|
||||
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')
|
||||
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', 0)
|
||||
size = data.get('size')
|
||||
if not name or not type_ or pose is None:
|
||||
raise ValueError('name, type, and pose are required')
|
||||
# 修改模型
|
||||
model = self.model_dict[name]
|
||||
if model.type != type_:
|
||||
raise ValueError(f'Cannot change model type from {self.model_dict[name].type} to {type_}')
|
||||
# 修改pose
|
||||
pass
|
||||
# 修改独特属性
|
||||
if type_ == 'box':
|
||||
if mass:
|
||||
model.mass = mass
|
||||
if tag_id:
|
||||
model.tag_id = tag_id
|
||||
elif type_ == 'table':
|
||||
height = position.get('z', 0)
|
||||
if height:
|
||||
model.height = height
|
||||
elif type_ == 'tag':
|
||||
if tag_id:
|
||||
model.tag_id = tag_id
|
||||
else:
|
||||
raise ValueError('Invalid type')
|
||||
self.gz_ctrl.delete_model(name)
|
||||
time.sleep(0.01)
|
||||
return self.post_scene_model_spawn()
|
||||
except Exception as e:
|
||||
return str(e), 400
|
||||
return 'OK', 200
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pass
|
||||
pass
|
||||
|
||||
@ -82,10 +82,14 @@ class SceneManager:
|
||||
print(f"process PID: {pid} already gone")
|
||||
except ValueError:
|
||||
print(f"invalid PID: {pid}")
|
||||
# os.remove(self.pid_file)
|
||||
os.remove(self.pid_file)
|
||||
self.pid_file = None
|
||||
print(self.sim_process.returncode)
|
||||
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 = None
|
||||
self.running_scene = None
|
||||
self.scene_plugin = None
|
||||
|
||||
Loading…
Reference in New Issue
Block a user