82 lines
3.3 KiB
Python
82 lines
3.3 KiB
Python
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)
|