242 lines
7.8 KiB
Python
242 lines
7.8 KiB
Python
import dataclasses
|
||
import importlib
|
||
import os
|
||
import socket
|
||
import subprocess
|
||
import threading
|
||
import time
|
||
|
||
import flask
|
||
import rospy
|
||
import werkzeug.serving
|
||
from flask import jsonify, request
|
||
|
||
import gazebo_ctrl
|
||
import scene_mgr
|
||
|
||
|
||
@dataclasses.dataclass
|
||
class HttpHost():
|
||
"""
|
||
HTTP主机信息
|
||
"""
|
||
hostname: str
|
||
port: int
|
||
|
||
def __post_init__(self):
|
||
if not self.hostname:
|
||
raise ValueError('Hostname cannot be empty')
|
||
if not (0 <= self.port <= 65535):
|
||
raise ValueError(f'Invalid port: {self.port}')
|
||
|
||
|
||
class HttpRPCServer():
|
||
"""
|
||
来自ability-sdk-python的http服务器。
|
||
HTTP RPC服务端实现。
|
||
使用add_route方法添加路由，重写类以实现自定义功能。
|
||
"""
|
||
|
||
def __init__(self):
|
||
self._server = None
|
||
self._app = flask.Flask(__name__)
|
||
|
||
def add_route(self, uri: str, handler, methods: list):
|
||
"""
|
||
添加HTTP路由，handler应符合Flask视图函数的规范。
|
||
"""
|
||
if not callable(handler):
|
||
raise ValueError('Handler must be a callable function')
|
||
self._app.add_url_rule(uri, view_func=handler, methods=methods)
|
||
|
||
def start(self, hostname: str = '0.0.0.0', port: int = None):
|
||
"""
|
||
启动并以阻塞方式提供服务。
|
||
"""
|
||
if self._server:
|
||
raise RuntimeError('Server is already running')
|
||
self.host = HttpHost(
|
||
hostname=hostname, port=port if port else self.find_free_port())
|
||
self._server = werkzeug.serving.make_server(
|
||
self.host.hostname, self.host.port, self._app, threaded=True)
|
||
self._server.serve_forever()
|
||
|
||
def stop(self):
|
||
"""
|
||
停止服务。
|
||
"""
|
||
if self._server:
|
||
self._server.shutdown()
|
||
self._server.server_close()
|
||
self._server = None
|
||
|
||
def find_free_port(self):
|
||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||
s.bind(('127.0.0.1', 0))
|
||
return s.getsockname()[1]
|
||
|
||
|
||
class GazeboSimHttpServer(HttpRPCServer):
|
||
"""
|
||
Gazebo仿真环境的HTTP服务器。
|
||
"""
|
||
|
||
def __init__(self):
|
||
super().__init__()
|
||
self.scene_mgr = scene_mgr.SceneManager()
|
||
# 启动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'])
|
||
|
||
# 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'])
|
||
|
||
# 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'])
|
||
|
||
# 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 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 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 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()
|
||
|
||
|
||
if __name__ == '__main__':
|
||
server = GazeboSimHttpServer()
|
||
server.start(hostname='0.0.0.0', port=12300)
|