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 0000000..5aa3965 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-0.png differ 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 0000000..adbbb84 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-1.png differ 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 0000000..aaa5f0e Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-2.png differ 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 0000000..aed9f95 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-3.png differ 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 0000000..559447d Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/april_36h11-4.png differ diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-0.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-0.png new file mode 100644 index 0000000..01d4fca Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-0.png differ 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 0000000..51962f3 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-1.png differ diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-2.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-2.png new file mode 100644 index 0000000..59ef498 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-2.png differ 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 0000000..5be8b45 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-3.png differ diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-4.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-4.png new file mode 100644 index 0000000..0b90b48 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-4.png differ 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 0000000..4406fe6 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-5.png differ 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 0000000..e5cdb0e Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-6.png differ 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 0000000..bea714e Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-7.png differ 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 0000000..3a76de1 Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-8.png differ diff --git a/scene/grasp-box/models/apriltag/materials/textures/tag36h11-9.png b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-9.png new file mode 100644 index 0000000..20e8d6e Binary files /dev/null and b/scene/grasp-box/models/apriltag/materials/textures/tag36h11-9.png differ 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()