diff --git a/1.txt b/1.txt deleted file mode 100644 index f5d78cd..0000000 --- a/1.txt +++ /dev/null @@ -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 - } -}' \ No newline at end of file diff --git a/gazebo_ctrl.py b/gazebo_ctrl.py index 3b5a2bc..d7c0d13 100644 --- a/gazebo_ctrl.py +++ b/gazebo_ctrl.py @@ -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) \ No newline at end of file diff --git a/http_server.py b/http_server.py index 1ef16bb..718f744 100644 --- a/http_server.py +++ b/http_server.py @@ -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: diff --git a/scene/grasp_box/gen_models.py b/scene/grasp_box/gen_models.py index 8503771..eeebb54 100644 --- a/scene/grasp_box/gen_models.py +++ b/scene/grasp_box/gen_models.py @@ -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), diff --git a/scene/grasp_box/launch.sh b/scene/grasp_box/launch.sh index 8c63800..97ab6b3 100755 --- a/scene/grasp_box/launch.sh +++ b/scene/grasp_box/launch.sh @@ -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" \ No newline at end of file +# 在这里阻塞 +wait \ No newline at end of file diff --git a/scene/grasp_box/pids.txt b/scene/grasp_box/pids.txt deleted file mode 100644 index 739754d..0000000 --- a/scene/grasp_box/pids.txt +++ /dev/null @@ -1,3 +0,0 @@ -23376 -23579 -24764 diff --git a/scene/grasp_box/plugin.py b/scene/grasp_box/plugin.py index a0c0aae..528d535 100644 --- a/scene/grasp_box/plugin.py +++ b/scene/grasp_box/plugin.py @@ -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 \ No newline at end of file + pass diff --git a/scene_mgr.py b/scene_mgr.py index 5f50451..189b2f9 100644 --- a/scene_mgr.py +++ b/scene_mgr.py @@ -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