64 lines
2.2 KiB
Python
Executable File
64 lines
2.2 KiB
Python
Executable File
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()
|