From 1843c219723857ca21c3cf9b1105b63c9c09ee5e Mon Sep 17 00:00:00 2001 From: zpff Date: Sat, 9 Aug 2025 22:00:02 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=AE=9E=E7=8E=B0=E5=9C=BA=E6=99=AF?= =?UTF-8?q?=E4=BF=AE=E6=94=B9=EF=BC=8C=E7=89=B9=E6=AE=8A=E7=89=A9=E5=93=81?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gazebo_ctrl.py | 169 +++++++++++ http_server.py | 73 +++++ main.py | 63 +++++ package.xml | 14 + scene/grasp-box/launch.sh | 3 + .../apriltag/materials/apriltag_0.material | 18 ++ .../apriltag/materials/apriltag_1.material | 18 ++ .../apriltag/materials/apriltag_2.material | 18 ++ .../apriltag/materials/apriltag_3.material | 18 ++ .../apriltag/materials/apriltag_4.material | 18 ++ .../apriltag/materials/apriltag_5.material | 18 ++ .../apriltag/materials/apriltag_6.material | 18 ++ .../apriltag/materials/apriltag_7.material | 18 ++ .../apriltag/materials/apriltag_8.material | 18 ++ .../apriltag/materials/apriltag_9.material | 18 ++ .../materials/textures/april_36h11-0.png | Bin 0 -> 1550 bytes .../materials/textures/april_36h11-1.png | Bin 0 -> 1567 bytes .../materials/textures/april_36h11-2.png | Bin 0 -> 1560 bytes .../materials/textures/april_36h11-3.png | Bin 0 -> 1555 bytes .../materials/textures/april_36h11-4.png | Bin 0 -> 1551 bytes .../materials/textures/tag36h11-0.png | Bin 0 -> 1708 bytes .../materials/textures/tag36h11-1.png | Bin 0 -> 1719 bytes .../materials/textures/tag36h11-2.png | Bin 0 -> 1721 bytes .../materials/textures/tag36h11-3.png | Bin 0 -> 1854 bytes .../materials/textures/tag36h11-4.png | Bin 0 -> 1892 bytes .../materials/textures/tag36h11-5.png | Bin 0 -> 1715 bytes .../materials/textures/tag36h11-6.png | Bin 0 -> 1727 bytes .../materials/textures/tag36h11-7.png | Bin 0 -> 1719 bytes .../materials/textures/tag36h11-8.png | Bin 0 -> 1717 bytes .../materials/textures/tag36h11-9.png | Bin 0 -> 1717 bytes .../models/apriltag/urdf/apriltag.xacro | 41 +++ .../models/apriltag/urdf/apriltag_cube.xacro | 51 ++++ .../models/apriltag/urdf/block.urdf.xacro | 93 +++++++ .../models/apriltag/urdf/grab_box_table.xacro | 263 ++++++++++++++++++ .../apriltag/urdf/standalone_cube.xacro | 232 +++++++++++++++ .../apriltag/urdf/table_apriltag_y.xacro | 262 +++++++++++++++++ scene/grasp-box/models/box.xacro | 105 +++++++ scene/grasp-box/models/fixed_tag.xacro | 26 ++ scene/grasp-box/models/table.xacro | 128 +++++++++ scene/grasp-box/plugin.py | 0 scene/grasp-box/scripts/gen_models.py | 81 ++++++ scene_plugin.py | 16 ++ 42 files changed, 1800 insertions(+) create mode 100644 gazebo_ctrl.py create mode 100644 http_server.py create mode 100755 main.py create mode 100644 package.xml create mode 100755 scene/grasp-box/launch.sh create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_0.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_1.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_2.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_3.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_4.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_5.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_6.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_7.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_8.material create mode 100644 scene/grasp-box/models/apriltag/materials/apriltag_9.material create mode 100644 scene/grasp-box/models/apriltag/materials/textures/april_36h11-0.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/april_36h11-1.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/april_36h11-2.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/april_36h11-3.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/april_36h11-4.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-0.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-1.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-2.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-3.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-4.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-5.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-6.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-7.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-8.png create mode 100644 scene/grasp-box/models/apriltag/materials/textures/tag36h11-9.png create mode 100644 scene/grasp-box/models/apriltag/urdf/apriltag.xacro create mode 100644 scene/grasp-box/models/apriltag/urdf/apriltag_cube.xacro create mode 100644 scene/grasp-box/models/apriltag/urdf/block.urdf.xacro create mode 100644 scene/grasp-box/models/apriltag/urdf/grab_box_table.xacro create mode 100644 scene/grasp-box/models/apriltag/urdf/standalone_cube.xacro create mode 100644 scene/grasp-box/models/apriltag/urdf/table_apriltag_y.xacro create mode 100644 scene/grasp-box/models/box.xacro create mode 100644 scene/grasp-box/models/fixed_tag.xacro create mode 100644 scene/grasp-box/models/table.xacro create mode 100644 scene/grasp-box/plugin.py create mode 100644 scene/grasp-box/scripts/gen_models.py create mode 100644 scene_plugin.py diff --git a/gazebo_ctrl.py b/gazebo_ctrl.py new file mode 100644 index 0000000..a7ea00b --- /dev/null +++ b/gazebo_ctrl.py @@ -0,0 +1,169 @@ +import rospy +from gazebo_msgs.srv import (GetLinkProperties, GetModelProperties, + GetModelPropertiesResponse, GetModelState, + GetModelStateResponse, GetWorldProperties) +from std_srvs.srv import Empty + + +class GazeboROSController: + def __init__(self): + rospy.init_node('gazebo_ros_controller', anonymous=True) + self.model_info = None + + def get_model_names(self) -> list: + rospy.wait_for_service('/gazebo/get_world_properties') + try: + get_world_properties = rospy.ServiceProxy( + '/gazebo/get_world_properties', GetWorldProperties) + world_properties = get_world_properties() + return world_properties.model_names + except rospy.ServiceException as e: + rospy.logerr(f"Service call failed: {e}") + return [] + + def get_model_state(self, model_names: list = None) -> dict: + """ + 获取模型的状态。 + 如果model_names为空，返回所有模型的状态，否则返回指定模型的状态。 + """ + if not model_names or len(model_names) == 0: + model_names = self.get_model_names() + print(f"Getting state for models: {model_names}") + model_states = {} + for name in model_names: + rospy.wait_for_service('/gazebo/get_model_state') + try: + get_model_state = rospy.ServiceProxy( + '/gazebo/get_model_state', GetModelState) + state = get_model_state(name, '') + model_states[name] = state + except rospy.ServiceException as e: + rospy.logerr(f"Service call failed for model {name}: {e}") + model_states[name] = None + return model_states + + def get_model_properties(self, model_names: list = None) -> dict: + """ + 获取模型的属性。 + 如果model_names为空，返回所有模型的属性，否则返回指定模型的属性。 + """ + if not model_names or len(model_names) == 0: + model_names = self.get_model_names() + print(f"Getting properties for models: {model_names}") + model_properties = {} + for name in model_names: + rospy.wait_for_service('/gazebo/get_model_properties') + try: + get_model_properties = rospy.ServiceProxy( + '/gazebo/get_model_properties', GetModelProperties) + properties = get_model_properties(name) + model_properties[name] = properties + except rospy.ServiceException as e: + rospy.logerr(f"Service call failed for model {name}: {e}") + model_properties[name] = None + return model_properties + + def get_link_properties(self, link_full_name: str) -> dict: + """ + 获取指定链接的属性。 + """ + rospy.wait_for_service('/gazebo/get_link_properties') + try: + get_link_properties = rospy.ServiceProxy( + '/gazebo/get_link_properties', GetLinkProperties) + properties = get_link_properties(link_full_name) + return properties + except rospy.ServiceException as e: + rospy.logerr(f"Service call failed for link {link_full_name}: {e}") + return None + + def format_model_state(self, state: GetModelStateResponse) -> dict: + return { + 'pose': { + 'position': { + 'x': state.pose.position.x, + 'y': state.pose.position.y, + 'z': state.pose.position.z + }, + 'orientation': { + 'x': state.pose.orientation.x, + 'y': state.pose.orientation.y, + 'z': state.pose.orientation.z, + 'w': state.pose.orientation.w + } + }, + 'twist': { + 'linear': { + 'x': state.twist.linear.x, + 'y': state.twist.linear.y, + 'z': state.twist.linear.z + }, + 'angular': { + 'x': state.twist.angular.x, + 'y': state.twist.angular.y, + 'z': state.twist.angular.z + } + }} + + def format_model_properties(self, properties: GetModelPropertiesResponse) -> dict: + return { + 'parent_model_name': properties.parent_model_name, + 'canonical_body_name': properties.canonical_body_name, + 'body_names': properties.body_names, + 'geom_names': properties.geom_names, + 'joint_names': properties.joint_names, + 'child_model_names': properties.child_model_names, + 'is_static': properties.is_static + } + + def format_model_info(self, model_name: str, state: GetModelStateResponse, properties: GetModelPropertiesResponse) -> dict: + return { + 'name': model_name, + 'state': self.format_model_state(state), + 'properties': self.format_model_properties(properties) + } + + def get_model_info_f(self, model_names: list = None) -> list: + """ + 获取模型的详细信息，包括状态和属性。 + 如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。 + 返回值已经格式化为list-dict形式。 + """ + if not model_names or len(model_names) == 0: + model_names = self.get_model_names() + print(f"Getting info for models: {model_names}") + model_info = [] + states = self.get_model_state(model_names) + properties = self.get_model_properties(model_names) + for name in model_names: + m_state = states[name] + m_properties = properties[name] + info = self.format_model_info(name, m_state, m_properties) + model_info.append(info) + return model_info + + def get_model_state_f(self, model_names: list = None) -> list: + """ + 获取模型的状态。 + 如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。 + 返回值已经格式化为list-dict形式。 + """ + if not model_names or len(model_names) == 0: + model_names = self.get_model_names() + print(f"Getting info for models: {model_names}") + model_info = [] + states = self.get_model_state(model_names) + for name in model_names: + m_state = states[name] + info = self.format_model_state(m_state) + info['name'] = name + # 获取箱子质量 + if name == 'block': + info['mass'] = self.get_block_mass() + model_info.append(info) + return model_info + +if __name__ == '__main__': + controller = GazeboROSController() + import json + print(json.dumps(controller.get_model_state_f(['block']))) diff --git a/http_server.py b/http_server.py new file mode 100644 index 0000000..4dfb597 --- /dev/null +++ b/http_server.py @@ -0,0 +1,73 @@ +import dataclasses +import socket + +import flask +import werkzeug.serving + + +@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__() \ No newline at end of file diff --git a/main.py b/main.py new file mode 100755 index 0000000..d4e2e28 --- /dev/null +++ b/main.py @@ -0,0 +1,63 @@ +import math +import os +import subprocess +import tempfile + +import rospy +from gazebo_msgs.srv import SpawnModel +from geometry_msgs.msg import Point, Pose, Quaternion +from tf.transformations import quaternion_from_euler + + +class GazeboWorldManager: + def __init__(self): + rospy.init_node('gazebo_world_manager') + self.spawn_service = rospy.ServiceProxy( + '/gazebo/spawn_urdf_model', SpawnModel) + + def spawn_apriltag_marker(self, model_name, tag_id, x=0, y=0, z=1): + """生成 AprilTag 标记模型""" + + xacro_file = "/home/leju/gazebo_world_manager/scene/grasp-box/models/fixed_tag.xacro" + + try: + # 添加包路径到 ROS_PACKAGE_PATH + 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 + + # 转换为 URDF + 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(0, 0, 0)) + ) + + # 生成模型 + 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}") + except subprocess.CalledProcessError as e: + rospy.logerr(f"Failed to convert xacro to urdf: {e}") + + +if __name__ == '__main__': + manager = GazeboWorldManager() + manager.spawn_apriltag_marker('mk', 0, 1, 1, 1.4) + # rospy.spin() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..39909fa --- /dev/null +++ b/package.xml @@ -0,0 +1,14 @@ + + + gazebo_world_manager + 0.0.0 + The gazebo_world_manager package + + user + TODO + + catkin + rospy + gazebo_msgs + geometry_msgs + \ No newline at end of file diff --git a/scene/grasp-box/launch.sh b/scene/grasp-box/launch.sh new file mode 100755 index 0000000..84ccfa4 --- /dev/null +++ b/scene/grasp-box/launch.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +# 启动仿真程序 \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_0.material b/scene/grasp-box/models/apriltag/materials/apriltag_0.material new file mode 100644 index 0000000..774adef --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_0.material @@ -0,0 +1,18 @@ +material AprilTag/Marker0 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-0.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_1.material b/scene/grasp-box/models/apriltag/materials/apriltag_1.material new file mode 100644 index 0000000..af11b15 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_1.material @@ -0,0 +1,18 @@ +material AprilTag/Marker1 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-1.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_2.material b/scene/grasp-box/models/apriltag/materials/apriltag_2.material new file mode 100644 index 0000000..7a3d440 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_2.material @@ -0,0 +1,18 @@ +material AprilTag/Marker2 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-2.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_3.material b/scene/grasp-box/models/apriltag/materials/apriltag_3.material new file mode 100644 index 0000000..2a95322 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_3.material @@ -0,0 +1,18 @@ +material AprilTag/Marker3 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-3.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_4.material b/scene/grasp-box/models/apriltag/materials/apriltag_4.material new file mode 100644 index 0000000..8681bff --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_4.material @@ -0,0 +1,18 @@ +material AprilTag/Marker4 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-4.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_5.material b/scene/grasp-box/models/apriltag/materials/apriltag_5.material new file mode 100644 index 0000000..63a6971 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_5.material @@ -0,0 +1,18 @@ +material AprilTag/Marker5 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-5.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_6.material b/scene/grasp-box/models/apriltag/materials/apriltag_6.material new file mode 100644 index 0000000..9e6d0c0 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_6.material @@ -0,0 +1,18 @@ +material AprilTag/Marker6 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-6.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_7.material b/scene/grasp-box/models/apriltag/materials/apriltag_7.material new file mode 100644 index 0000000..7d8d608 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_7.material @@ -0,0 +1,18 @@ +material AprilTag/Marker7 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-7.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_8.material b/scene/grasp-box/models/apriltag/materials/apriltag_8.material new file mode 100644 index 0000000..ffdd672 --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_8.material @@ -0,0 +1,18 @@ +material AprilTag/Marker8 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-8.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/apriltag_9.material b/scene/grasp-box/models/apriltag/materials/apriltag_9.material new file mode 100644 index 0000000..461404c --- /dev/null +++ b/scene/grasp-box/models/apriltag/materials/apriltag_9.material @@ -0,0 +1,18 @@ +material AprilTag/Marker9 +{ + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.0 0.0 0.0 1.0 + texture_unit + { + texture textures/tag36h11-9.png + filtering trilinear + scale 1.0 1.0 + } + } + } +} \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/materials/textures/april_36h11-0.png b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-0.png new file mode 100644 index 0000000000000000000000000000000000000000..5aa39653bf1f55dd74f37e9ef69d96549e5f85d4 GIT binary patch literal 1550 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu$}hvvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHAL|XlWT_<)mvo5u8S>Y$<;wrP1%XNi7R%@87PuwqdU#UqV;_e5xK6r3G z>FU9wk>Tv&jskxe3&rP%#4b7Vdy-SPgnZ1sDmB-y+cMSc@BXOszq)>1oN2nDf`ZU_qr+P|%{!Kx=Qe+~ zE1V&kI`zk#iZe^tI^}n+tN3$l=YvfLj1Q+-nflA9iE-zy*2&IWcudG_eSBtgs!z1jxLoDV)H0!SVvCF_{VN3FMcVYMsf(!O8p9~b? zEbxdd2FBoZ5N34Jm|X!BWH0gbb!C6aEXAzE8+UIdD+2=yx2KC^NX4zU*BW`59C?@z zO2q!xoh|0@$gwg_%VK8w)H6Qp4f88!FgSEDwn#82vI!(Hv`7FsKw6QFW`YgZ=jQU~ zy$;_UA#}f`HvIqdZMkohKz7s6PIS*b;7z~XJb6u3{_RrvqsPpfyQ%Gc!a?~!x{vdQ zUZ>!j*Gkk5YXXt^Abt)o0(QUJ>v`;0LpRY8PMzp!;6Hih3eWY-?`LNpIkx7nT+9&$ z4#NfkW`!dRPCP)G!w^UVIV1`aukFqq!-HEVNig1L(IYy^5eQJ?v&;iEBV$u$5(1q% d`LRKC9_LAg^H&^=w*pHF22WQ%mvv4FO#o6o!@U3i literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/april_36h11-1.png b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-1.png new file mode 100644 index 0000000000000000000000000000000000000000..adbbb84fcfd34987f67bde8e8b2003aef154bf6b GIT binary patch literal 1567 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu$}hvvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvPzSR{X^G4yQqI2mc1+7ObLN%OP1GKliN+^0MZj$K{6(hrXy{TEg zJCt2KJyte`)$s%Kk=TNul8Hk9r;9`#sVKP9U;bUL{5`|M$i!Kq66_{>)0R!b(Aga+d-V$Eh<$&r~#7&wniCmydt{$maC#JM$OEpN;1E(0OF@8H4O2(nl`OHf{cF zSJ*N&W$KSP6=#-k3)vUN7XIulzL?mU?smqfYMzCYj@a&qmq}qprjIlq%~}-R!PO0K-zVSBo6GPo{ejo}AFF*6 zo=yJZ=H4yK$b9?2(Ng!|Zw;qq6JDI0;Cpl8ACp!m#tQqz`KK70w>#XQbvwFRemTEG z+b*xW4D9U(GA3@iZhPjG918YVN3FMcVYMsf(!O8p9~b? zEbxdd2FBoZ5N34Jm|X!BWH0gbb!C6aEXBsk*}7MD5(5K^jHioZNX4zU*BUdK19@B= zb9elUzPCkyz2|1ivZj8&?*Z@Du^%{IBgxnxz|3LD;KZYFgn`2l$N|z$JhT(E|Gvg{ z$J+JZ(mda=Z2tW~iOr#du|{%)N&f8OiW8~HqJruVk%7bbD+ zATP}E*hj$aKlF>rW2*{ouVqv(_TFgrc<21Q9CfkpiMhw<9v5*R*4SEfu5X)rz?Ft+ y9+A0Fa@q&Mox7!&u}44ISpiS9K+SC6ysz>_>G+Biy(nPO!QkoY=d#Wzp$P!WlhE-1 literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/april_36h11-2.png b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-2.png new file mode 100644 index 0000000000000000000000000000000000000000..aaa5f0efeb96e86f540bb07efc22477d019fec66 GIT binary patch literal 1560 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu$}hvvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvP&T?JFS0+$+B4DmgN0&vGXjg#Z6|S{bGh%)znq+ze#mKN;Z)%qB z4rLclkClyKb^O45B(@-^WTMdj=^{}_DhlrOmw%Tlf6uTmE^U^m6tl@)v&$!2W}IEy z@i4sQLbsobz=WBpXS{;A_C0wK9FEbGtu<`B#49yU_L9pV|4z zH}I5B-Nneq*Emrrb^XshyI1esUVq-?_ix6_8Aa(&qP@QWqlGQW+ueoXKL{?^yL>WG zgtNdSvKSbH*Fl)kNn>^eP>{XE)7O>#B?})LkD!)^QYZrhi>RlIV@SoVx7Q54SONrG z4(5IQ7oI2F%*zuJlX7E~aLqIR{>(|N4fUVXSQwmm6pk=(7&Ztn18FB7AkAS&JHeX! zzqhjIf6n&5cFixLJ@yF0dgl(t772$AMu8*-MK&PaA_1g<9NGvzn5H(<(B^zu{+`_0 zIdeDg>(Rwl6z?_Yo1E0Mt+>6Gar-u&0zqPfn@X|tA$(TKu>*_WUh~@WHS^oUFMd1d z8vekL_|XWAf`Xf6*{!#KnZ#gAbF@r=f5Il8k*L@b$tkCFQL3q%ekl)@1|9?h9l(Iq g-?Yqtb3QO~3##l~bSuycSY|MIy85}Sb4q9e05g-#?*IS* literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/april_36h11-3.png b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-3.png new file mode 100644 index 0000000000000000000000000000000000000000..aed9f95695896f04479228364e6c46cea23e48a9 GIT binary patch literal 1555 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu$}hvvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvP&T?JFS0+$+B4DmgN0&vGXjg#Z6|S{bGh%)znq+ze#mKN;Z)%qB z4rLclkClyKb^O45B(@-^WTMdj=^{}_DhlrOmw%Tlf6uTmE^U^m6tl@)v&$!2W}IEy z@i4sQLbsobz=WBpXS{;A_C0wK9FEbGtu<`B#49yU_L9pV|4z zH}I5B-Nneq*Emrrb^XshyI1esUVq-?_ix6_8Aa(&qP@QWqlGQW+ueoXKL{?^yL>WG zgtNdSvKSbH*Fl)kNn>^eP>{XE)7O>#B?})Lk1F?{YY!M0SOh#>978H@y}jnx$K=Sv z;wb*=fBjC2!<;H@M{jkAN6ecy#n@Pu;oq6FTnsG|4jqgFNeqf?K)OW&NCP=E6Rfy< z)2tvmzL(uo=b-)eU%op)cGA@Whk*_d0y-d9mr>l1!HGxV2m^;v;+UiM5QdxkNeqgXVFrifd(8_juNryg%qlsc+HkBoJ~3>I>IA zh|0ZwcFvLZ&uV+v#ih-1w(J_i j10|YfG0eQ2&}uCqr*Le}$|XO5#RY?>tDnm{r-UW|{Taxw literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/april_36h11-4.png b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-4.png new file mode 100644 index 0000000000000000000000000000000000000000..559447d6688b67f7e3757d84f694dacf1fd45861 GIT binary patch literal 1551 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu$}hvvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvP&T?JFS0+$+B4DmgN0&vGXjg#Z6|S{bGh%)znq+ze#mKN;Z)%qB z4rLclkClyKb^O45B(@-^WTMdj=^{}_DhlrOmw%Tlf6uTmE^U^m6tl@)v&$!2W}IEy z@i4sQLbsobz=WBpXS{;A_C0wK9FEbGtu<`B#49yU_L9pV|4z zH}I5B-Nneq*Emrrb^XshyI1esUVq-?_ix6_8Aa(&qP@QWqlGQW+ueoXKL{?^yL>WG zgtNdSvKSbH*Fl)kNn>^eP>{XE)7O>#B?})LkD;I%(_97y79LL*$B>F!Z?9SNF*)+M zIGWG;9lz7l9 z-#(DLSIU3VkG8khU3nS=m^lm?oOl$DFmMUO#+uz5clp+YUZ(nA0l)8e-3FYn)tR8dfiy`&L{WTYys% z`nbFeGxPnaGbNV~~`p9wT~IZTTZo{(9nLyXD#Ca lS8647LV*pwap1s{?( zzIyOz&FbdW3LJF}JGlM2^nxD!pXAgn!Qc1$+aEI5KE6-gFInRGA<(H3tpJa3T_nrBR-A~Ts`M`N(vyZvXBi=_ZeZ8OW z@s~+-^;EP|mot9RbfiCKdTia}n1UPUjg{T3O#NjP#kg{Hwi{|oX?+wa3<)$n^*uc% z%p$)q>~!e0J?iy|7R%S_6wiGoAQW*#$c1Atl!G-MRHW3=3ly1a5x&KKXXu9EQg92VU7fwM!G8 zP5u()-7Uvhd;7rAQt!#%8&1n6Jn)>(b8;e+$={zgAL`uhPhtI)-}ugS`}Svce)0__ zrBnAX^6@oZWJ+CMy=V98z1!>0oBaOG827Vie&4V1W58%(OY(MiVfYV%3-&Ib3>4uk z@Q5r1#^7}jW^~e+T>%tiFY)wsWq--S$EK*ahb6R~fq`YEr;B4q#jUs3FLE9>5O6t| zv*Ta%y&4r(iAKkX=~@EWXC7bLrqA%BayA1)B7=hjg8&;t3nTGN5|y_wHn7i}D?j~+ zI8)!1YeyLl)RSxg#BC(H)Z)&J1m9?79gjz|Z=KE4!|F#8jf6QIUs&BZ2DElk?)_`m zZZn88GaO=QFkn#NVc;OdBt{JuLm;Nd7^pR@S8>}Qjoq6tcjNRfP6?bk<ExVETk2Qu+uo4;c=x)M@w1R%agO!sc z4w|#lJ4Q)LLP-7~CQOl(1DX8B*7obx>a#pZB2*-hQdS_TMNdi%w`&;AL~1DC(AXme QEXNo;UHx3vIVCg!0O2O6T>t<8 literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-1.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-1.png new file mode 100644 index 0000000000000000000000000000000000000000..51962f3d2f7d07be8520dd19f8d2825975177d8d GIT binary patch literal 1719 zcmeAS@N?(olHy`uVBq!ia0y~yU~B_n4mO}jeS_LlAg3kM**U<|*;%2WC_gPTCzXLg zV`A+@TaUvIGDqX>wap1s{?( zzIyOz&FbdW3LJF}JGlM2^nxD!pXAgn!Qc1$+aEI5KE6-gFInRGA<(H3tpJa3T_nrBR-A~Ts`M`N(vyZvXBi=_ZeZ8OW z@s~+-^;EP|mot9RbfiCKdTia}n1UPUjg{T3O#NjP#kg{Hwi{|oX?+wa3<)$n^*uc% z%p$)q>~!e0J?iy|7R%S_6wiGoAQW*#$c1Atl!G-MRHW3=3ly1a5x&KKXXu9EQg92VU7fwM!G8 zP5u()-7Uvhd;7rAQt!#%8&1n6Jn)>(b8;e+$={zgAL`uhPhtI)-}ugS`}Svce)0__ zrBnAX^6@oZWJ+CMy=V98z1!>0oBaOG827Vie&4V1W58%(OY(MiVfYV%3-&Ib3>4uk z@Q5r1#^7}jW^~e+T>%tiFY)wsWq--S$EL(-q`X<5fq`XMP1@9TwaEm?SEH@bDN*`Yox44npRo!F#aUL!yar$Kr8%La*V$ znJU@)*RExL_hA=7NjieS|&|CL#p4(#DbT@DE5snH&0+C95Y$MqYWTNs8 zF&tn82J)mef46Pp`?6tn6EJr1rzm_Lz@rl*k;WwZ6fAptE8jcG+>OW(F0VNtvEZes@zjp) znQyuzs$3cFawJJCHrm`1>FIT-xaRKRK?}wL_C-&Xgqh8a-TqVt6A8s8KS3$jg3ND9T!(kPEvIJUC3X*|MBz_ zODAMA_sD$KaBhC{C%K>lfOiHcgr!_-ac@&)I0cF!)e(C?@vzfy*crZNo$QkNGugu^e!63lK(89=&NCJ~Y<%tXr`p=x} z_fB#?F!9zd35FlMhZq_R7!-IIIG7noVUno4U0ONf@-+c5m5srF%*C-Fue3XO)MPe#~ z1vDh7%FVdQ&MBb@02isd@c;k- literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-3.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-3.png new file mode 100644 index 0000000000000000000000000000000000000000..5be8b459bb6d3302ba02f130b9c9f1aaecd0efb7 GIT binary patch literal 1854 zcmeAS@N?(olHy`uVBq!ia0y~yU`zsG4mP03nV3seKu&9>vvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvPp4Js2`$pl>goV2@Ttkiot`2D7S{Rmb_283B{0lvuFFkssu|rJe zPsAFl9Tu-&J<#H4l2>@wGH7O@Q+oJt}g8ykhRIxeo7oTTXbyO6(r|KsT= zmQKiK?veSd;oSV@$HD&(%D0ErCEfAmI^Ou8JGY*PX<_Vwz|C*pC*RJS!|*Ttfmi;| z)1?W&CVz?Y?v`V;y?x+lsdw&6h2cL4F4((# zGEjuGz$3C47=_nCn9)gNb_Gz7y~NYkmHj0PAFGZMQ`F5J3=FJRo-U3d6}R5rSXlJR zL8Rf~*|{qcK5jkgRB~_OjiiOUW~pm#%#dTco6zhk&RzDK>E)Tdhrg6fnY|s}7|YZS3GFiB0E}yz_A1`@>(W zN)4R;d{>+7Sy5lVy@N-@H4!AUYu(?>x`UTl8U&a*3>lnw6pk=(7y>y!+KGp9!6$3C zG$q#FZ{M{}SBW|HdgP&oRjYIyJ3g1~jr)7R*l@0&ZsVHDSqxMPbXX98BXUPzChOs2 zj|*pCHTJ(=>cq2u%5qNX$3D~{@6KdxV~I}A9t^)tSr=E7VCD+n5EVksW;s_ z=y{3N+!z7HbGI*F4mR4nC0F{|>#fd`7fgI3^rm~BDYB3`^!oK`=aUVz36KWqsbL0h zigvafemLQbO@Dv?!d0tO(j@zjCz{Px-8_5U>q#@8mRPA4=l|OG-T9o0U~vvYu_v$H}$QGQxxPAUU~ z#>Co*wjPHaWRAvPp4Js2`$pl>goV2@Ttkiot`2D7S{Rmb_283B{0lvuFFkssu|rJe zPsAFl9Tu-&J<#H4l2>@wGH7O@Q+oJt}g8ykhRIxeo7oTTXbyO6(r|KsT= zmQKiK?veSd;oSV@$HD&(%D0ErCEfAmI^Ou8JGY*PX<_Vwz|C*pC*RJS!|*Ttfmi;| z)1?W&CVz?Y?v`V;y?x+lsdw&6h2cL4F4((# zGEjuGz$3C47=_nCn9)gNb_Gz7y~NYkmHj0PAFH0?%G6g43=FIZo-U3d6}R5rFbrDa zAky$~o~}}HP?Gvo&$*K)ESjz$!{6AJyrw+iQf5%6r|Hotdl}f)y}os&?mlPS-gmzi zRvDlFEW_}_aQ_+xhYrRT2?j+rfh2|&2_OeZE3#287^dr;Ipg^8;}=Tj1~cxvTDPA+ zG?eq>q;;bo+yDBkhQ?!a~7qx7WI^ap*Ce8QHanqjH|Q z;b{}!LBLtE~9`RSqVxUsmK#Q}6($i}aB6|#T%%r;`8mF9l`R||I zmD0IKbZYOnZ_W@e<{*m$MHXhxN_l1}!@vY;WY0#D6Irbv>jZ?VfZJ0)-q0@V+_6Te_yEM-M)!wo*GqiFCvB#?gDeu zw@;sh%67*FeV(xI@I!%H+kBIzWzLf65Lkcv_wVWs!zh;dMt#W=$1f`V5TRMpU21Zs uZhyJbf%@yS&f1*coXq&cl7^nHWB!ma<5uVCU9W(}FN3G6pUXO@geCwAdWD|= literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-5.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-5.png new file mode 100644 index 0000000000000000000000000000000000000000..4406fe6d492b59081397fda866c2e7c6bf79ae46 GIT binary patch literal 1715 zcmeAS@N?(olHy`uVBq!ia0y~yU~B_n4mO}jeS_LlAg3kM**U<|*;%2WC_gPTCzXLg zV`A+@TaUvIGDqVtdxdK8y;XQLVPRJ@YoMU}S|>rRDNzeU*(ZDNU*PFH>Cq#N9bz(n zBGy>#uz3CIffh%Tyu!Pt$&0$D{Mf(bg3__($8~#lf8SBf&e6*ieD;9Ff~;v1{f->X z$qVJ#r_^9jWzaR_h)=EFRIfwDHFpmmzrI`I{3pBb&tC+8Qe&=B`lxf6x4WXbqU!P` z%lE?W5uQOE=R1${>{9rsd_8sgeG73}v5K@KCzd?#R6dflvH2-qnyRzvJf%LZRc=3> zuBEnq7E9k6l)Se3lX%;wTc?b!XYwgFy6Pyh3L0f;EN0?1j4}=KS(0i0?xB3iAI+kZ zPqdmRCNqYvm>^^SL;laZYdSuDJ3JR!T=<%FTmG;^fq2HX+~2=%Y@4m#@LzJrrCa~9 zm}mS{*?aHWB6o-XW(9?JuRXDSP{b}#aY@y=NX6cFVF1Iu^Dkx>Is6cN5nEiH{r2$} z#uEYIC#4@e5RlLe`nK!%?%meazrXve-Y-AvWzqI+JPxOU(ZZJG?e4mbbNq%pe!D9B#o>Fdh=l0}$P!|I;#r!x!;EE_ys978H@y}f>s_mF{r zi=$ZVe_eg~fX1aR8)vRmh-#jvm;0NkVg7bb1_3sP7Dk3d1_ue^nItL)8vR50^OJt> zN39LYTT2ZYHn5XsAKY!ExwPSMFXy!_rcB#ClUQPS7&w?24ly(sFeqSU;*`Ly943lk zWI>}LW4TvS%i%3wtBVccmSL!a89=~Z{Emhha$wsrmJe62ad1zPICvD{N8&VNxBzA< zh6Jj4x21&VsGm&Uzjp0izT0PK??NOX($fbd6%rS{3F*mArk*_oGq$;Xh(CbjT|BXi z;U!eB5TWzHd`aOonZIx3dp~OAd+b|?h+6EyM6C!vV0w%t8yKy550Vghl(>L`XB${< rBQCEuoIN|)e%)GerUJV8G1V@eNmXs>zvJ$};*7!5)z4*}Q$iB}Kw!Mk literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-6.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-6.png new file mode 100644 index 0000000000000000000000000000000000000000..e5cdb0ebff468cae62389a8c5722154945cdd024 GIT binary patch literal 1727 zcmeAS@N?(olHy`uVBq!ia0y~yU~B_n4mO}jeS_LlAg3kM**U<|*;%2WC_gPTCzXLg zV`A+@TaUvIGDqVtdxdK8y;XQLVPRJ@YoMU}S|>rRDNzeU*(ZDNU*PFH>Cq#N9bz(n zBGy>#uz3CIffh%Tyu!Pt$&0$D{Mf(bg3__($8~#lf8SBf&e6*ieD;9Ff~;v1{f->X z$qVJ#r_^9jWzaR_h)=EFRIfwDHFpmmzrI`I{3pBb&tC+8Qe&=B`lxf6x4WXbqU!P` z%lE?W5uQOE=R1${>{9rsd_8sgeG73}v5K@KCzd?#R6dflvH2-qnyRzvJf%LZRc=3> zuBEnq7E9k6l)Se3lX%;wTc?b!XYwgFy6Pyh3L0f;EN0?1j4}=KS(0i0?xB3iAI+kZ zPqdmRCNqYvm>^^SL;laZYdSuDJ3JR!T=<%FTmG;^fq2HX+~2=%Y@4m#@LzJrrCa~9 zm}mS{*?aHWB6o-XW(9?JuRXDSP{b}#aY@y=NX6cFVF1Iu^Dkx>Is6cN5nEiH{r2$} z#uEYIC#4@e5RlLe`nK!%?%meazrXve-Y-AvWzqI+JPxOU(ZZJG?e4mbbNq%pe!D9B#o>Fdh=l0}$PQ@rW>{2vSqEPFg%978H@y}j<3#cU|x z5;*zI|zZn{~FXv_oKa|ox zJ)E>dF=6SpI39)?Hqz{ayNxuLHazX)Z1|sH{Vm5cW`Z~~!y$$S0|o^i1`a|@V$@(U z1Y-K1S2GgkZ%aNu?MU8fpMAl5Xd14N*aHX41O4MH*SE-4Z&N9Y-Zs1b;M^)h0-hy4 zKu|r4$~(kxz_yQbou|>aoZQ$8o9?RB=T{(Xrj{MpWA1>uwb~8!PQhuJud5Gt6H3P9 zBz0iYgQq@NqRlJtVbIS#K2Nvv?zFehj$%z!=wXE2{TQP7Ql7Dl@BwcjEWMo-v r{veo}_MMqovtwHx#{v2U?Pq4IS9%JO>p44sMH_>stDnm{r-UW|mwvp> literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-7.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-7.png new file mode 100644 index 0000000000000000000000000000000000000000..bea714e48360144c59b703e0d8ffc387366b441f GIT binary patch literal 1719 zcmeAS@N?(olHy`uVBq!ia0y~yU~B_n4mO}jeS_LlAg3kM**U<|*;%2WC_gPTCzXLg zV`A+@TaUvIGDqVtdxdK8y;XQLVPRJ@YoMU}S|>rRDNzeU*(ZDNU*PFH>Cq#N9bz(n zBGy>#uz3CIffh%Tyu!Pt$&0$D{Mf(bg3__($8~#lf8SBf&e6*ieD;9Ff~;v1{f->X z$qVJ#r_^9jWzaR_h)=EFRIfwDHFpmmzrI`I{3pBb&tC+8Qe&=B`lxf6x4WXbqU!P` z%lE?W5uQOE=R1${>{9rsd_8sgeG73}v5K@KCzd?#R6dflvH2-qnyRzvJf%LZRc=3> zuBEnq7E9k6l)Se3lX%;wTc?b!XYwgFy6Pyh3L0f;EN0?1j4}=KS(0i0?xB3iAI+kZ zPqdmRCNqYvm>^^SL;laZYdSuDJ3JR!T=<%FTmG;^fq2HX+~2=%Y@4m#@LzJrrCa~9 zm}mS{*?aHWB6o-XW(9?JuRXDSP{b}#aY@y=NX6cFVF1Iu^Dkx>Is6cN5nEiH{r2$} z#uEYIC#4@e5RlLe`nK!%?%meazrXve-Y-AvWzqI+JPxOU(ZZJG?e4mbbNq%pe!D9B#o>Fdh=l0}$PQ`Jl198id5i>HfYNX4zU*B!Z-0(o2< zql^A)Z|A;i&EfPXZ0>`}XFhn``o_kfU#`sHAi*HO#?ZpZkVpcPMCFML5BkqMJFdD> z@qtHfEDysUHc~7@QcIe>4TpOL4+P)dn(v+DxPy>4N%B7)Cq3Xk#*wgpTlW2FN19@I z7&w?24ly(sfRF-$M>G?nnV|9yn~!mvyIl76_N0gjJ0B)tjY-nN4jvqM5=VpSF%CPh ztuto7Kl{C{n}kG!$5^=4SX>NAt$Gs5KYWCoEpqa5bzf}Q`O_bmreFb0Fsvwwi4W%_ z4p!b;R~s$FZDfuVy-2oFWImEPNX!k#fVNJOXxiCGzaXA*wm&=DnL&?G*+5qLk;s4% d$v|}Rx^e&j literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-8.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-8.png new file mode 100644 index 0000000000000000000000000000000000000000..3a76de12228f9cffb612fcfdfb73becbf98f0aa7 GIT binary patch literal 1717 zcmeAS@N?(olHy`uVBq!ia0y~yU~B_n4mO}jeS_LlAg3kM**U<|*;%2WC_gPTCzXLg zV`A+@TaUvIGDqVtdxdK8y;XQLVPRJ@YoMU}S|>rRDNzeU*(ZDNU*PFH>Cq#N9bz(n zBGy>#uz3CIffh%Tyu!Pt$&0$D{Mf(bg3__($8~#lf8SBf&e6*ieD;9Ff~;v1{f->X z$qVJ#r_^9jWzaR_h)=EFRIfwDHFpmmzrI`I{3pBb&tC+8Qe&=B`lxf6x4WXbqU!P` z%lE?W5uQOE=R1${>{9rsd_8sgeG73}v5K@KCzd?#R6dflvH2-qnyRzvJf%LZRc=3> zuBEnq7E9k6l)Se3lX%;wTc?b!XYwgFy6Pyh3L0f;EN0?1j4}=KS(0i0?xB3iAI+kZ zPqdmRCNqYvm>^^SL;laZYdSuDJ3JR!T=<%FTmG;^fq2HX+~2=%Y@4m#@LzJrrCa~9 zm}mS{*?aHWB6o-XW(9?JuRXDSP{b}#aY@y=NX6cFVF1Iu^Dkx>Is6cN5nEiH{r2$} z#uEYIC#4@e5RlLe`nK!%?%meazrXve-Y-AvWzqI+JPxOU(ZZJG?e4mbbNq%pe!D9B#o>Fdh=l0}$P)9l>5{$&gdESo%C978H@y}jDQE zCoiLW*6SpO2lk{`hNPB6du8%^8sf8dzr8&vLSTmkg8&;t3nN1!g98an5|smu-Z9g# z$uxI+ZL|>gr@as33k-=5T;d}DDNKM&Fz*0sGE4R(i9?CS+1ZW{l1TC;JVZzip9g8j zIL=+ZCcq$0)8qqAq>ywPW8k)7*;+*zwa)i%H|%^!ibqNI9VA@9>88OS=+Vh3x3_Qo zYG!R}j@`Q?rcYQb;>&}6grRDNzeU*(ZDNU*PFH>Cq#N9bz(n zBGy>#uz3CIffh%Tyu!Pt$&0$D{Mf(bg3__($8~#lf8SBf&e6*ieD;9Ff~;v1{f->X z$qVJ#r_^9jWzaR_h)=EFRIfwDHFpmmzrI`I{3pBb&tC+8Qe&=B`lxf6x4WXbqU!P` z%lE?W5uQOE=R1${>{9rsd_8sgeG73}v5K@KCzd?#R6dflvH2-qnyRzvJf%LZRc=3> zuBEnq7E9k6l)Se3lX%;wTc?b!XYwgFy6Pyh3L0f;EN0?1j4}=KS(0i0?xB3iAI+kZ zPqdmRCNqYvm>^^SL;laZYdSuDJ3JR!T=<%FTmG;^fq2HX+~2=%Y@4m#@LzJrrCa~9 zm}mS{*?aHWB6o-XW(9?JuRXDSP{b}#aY@y=NX6cFVF1Iu^Dkx>Is6cN5nEiH{r2$} z#uEYIC#4@e5RlLe`nK!%?%meazrXve-Y-AvWzqI+JPxOU(ZZJG?e4mbbNq%pe!D9B#o>Fdh=l0}$Pi|_2ll-~>tESo%C978H@y}j-0i0N+(!Vb*5rDq*2RLx9^rAP*nZa=?qSb@)^ z)b#A^{D0CrNJvDar#VRAgHvll`$nb*;UOdh84?w=U|$j+6G(vqWP*7I!joB# zZ`sQ7XM65#V0uCdV$y>RoFa%!6fxH#4jOvuF#hMIpOtsk*u+HZvph&5R4|ZJcwos> c4f`vZeXeRNG^%k20}C_;Pgg&ebxsLQ0Aa1Om;e9( literal 0 HcmV?d00001 diff --git a/scene/grasp-box/models/apriltag/urdf/apriltag.xacro b/scene/grasp-box/models/apriltag/urdf/apriltag.xacro new file mode 100644 index 0000000..df7f37e --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/apriltag.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + __default__ + + false + + + + + diff --git a/scene/grasp-box/models/apriltag/urdf/apriltag_cube.xacro b/scene/grasp-box/models/apriltag/urdf/apriltag_cube.xacro new file mode 100644 index 0000000..86d0f44 --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/apriltag_cube.xacro @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/urdf/block.urdf.xacro b/scene/grasp-box/models/apriltag/urdf/block.urdf.xacro new file mode 100644 index 0000000..1b5cd41 --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/block.urdf.xacro @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + 10 + + 1e6 + 100 + + + + + + + + + + + + + + + + + + + + + 10 + + 1e6 + 100 + + + + + + + + + + + + + + + + + + + + + 10 + + 1e6 + 100 + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/urdf/grab_box_table.xacro b/scene/grasp-box/models/apriltag/urdf/grab_box_table.xacro new file mode 100644 index 0000000..385a449 --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/grab_box_table.xacro @@ -0,0 +1,263 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/urdf/standalone_cube.xacro b/scene/grasp-box/models/apriltag/urdf/standalone_cube.xacro new file mode 100644 index 0000000..5b4c735 --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/standalone_cube.xacro @@ -0,0 +1,232 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/apriltag/urdf/table_apriltag_y.xacro b/scene/grasp-box/models/apriltag/urdf/table_apriltag_y.xacro new file mode 100644 index 0000000..069ba48 --- /dev/null +++ b/scene/grasp-box/models/apriltag/urdf/table_apriltag_y.xacro @@ -0,0 +1,262 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/box.xacro b/scene/grasp-box/models/box.xacro new file mode 100644 index 0000000..f8acf75 --- /dev/null +++ b/scene/grasp-box/models/box.xacro @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + + 1e6 + 100 + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + + 1e6 + 100 + + + + + + + + + + + + + + + + + + + + + + diff --git a/scene/grasp-box/models/fixed_tag.xacro b/scene/grasp-box/models/fixed_tag.xacro new file mode 100644 index 0000000..5043511 --- /dev/null +++ b/scene/grasp-box/models/fixed_tag.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/models/table.xacro b/scene/grasp-box/models/table.xacro new file mode 100644 index 0000000..c9d77ec --- /dev/null +++ b/scene/grasp-box/models/table.xacro @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + 1000000.0 + 100.0 + 0.001 + 1.0 + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Wood + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scene/grasp-box/plugin.py b/scene/grasp-box/plugin.py new file mode 100644 index 0000000..e69de29 diff --git a/scene/grasp-box/scripts/gen_models.py b/scene/grasp-box/scripts/gen_models.py new file mode 100644 index 0000000..fd5d923 --- /dev/null +++ b/scene/grasp-box/scripts/gen_models.py @@ -0,0 +1,81 @@ +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): + rospy.init_node('gazebo_world_manager', anonymous=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}") + except subprocess.CalledProcessError as e: + rospy.logerr(f"Failed to convert xacro to urdf: {e}") + + def generate_table(self, name, x, y, height): + 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=0), + orientation=Quaternion(*quaternion_from_euler(0, 0, 0)) + ) + 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)) + ) + 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_weight:={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)) + ) + self._generate(name, urdf_content, pose) + + +if __name__ == '__main__': + generator = GraspModelGenerator() + generator.generate_apriltag("tg2", 0, 2, 0, 1.1, 0, 1.57, 1.57) diff --git a/scene_plugin.py b/scene_plugin.py new file mode 100644 index 0000000..d9c5f58 --- /dev/null +++ b/scene_plugin.py @@ -0,0 +1,16 @@ +class ScenePlugin: + """ + 场景自定义模型的处理。 + """ + + 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()