feat: 实现一般模型修改，诡异程序停止、场景模型修改

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
-    }
-}'

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)

http_server.py

@@ -140,9 +140,18 @@ class GazeboSimHttpServer(HttpRPCServer):
 
     def shutdown_simulation(self):
         try:
-            self.scene_mgr.shut_scene()
+            if not self.scene_name:
-            self.gazebo_ctrl = None
+                return 'No scene is running', 400
-            self.scene_name = None
+            # 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 'Missing required field name', 400
-        return jsonify({'status': 'model state updated', 'data': data})
+        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:

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

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"
+wait
-# 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"

scene/grasp_box/pids.txt

@@ -1,3 +0,0 @@
-23376
-23579
-24764

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')
+            self.gz_ctrl.delete_model(name)
-            pose = data.get('pose')
+            time.sleep(0.01)
-            position = pose.get('position')
+            return self.post_scene_model_spawn()
-            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')
         except Exception as e:
             return str(e), 400
-        return 'OK', 200
+
 
 if __name__ == '__main__':
-    pass
+    pass

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