实验目的：
写一个Python程序，在ROS melodic Python2.7 环境的已经开启了的gazebo中某个指定坐标生成名为aruco_cubo_hover的模型，并以一个指定的速度朝某个指定方向运动一段时间
同时这个模型我已经将其中的重力标签<gravity>部分改为0，所以希望在运动结束后，停止运动并删除这个模型
程序内容：
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import os import math from geometry_msgs.msg import Pose from gazebo_msgs.msg import ModelState from gazebo_msgs.srv import SpawnModel, DeleteModel def generate_model_name(model_name): """ 生成模型的命名空间 """ return "aruco_cubo_hover_{}".format(model_name) def spawn_model(model_name, model_path, pose): """ 在Gazebo中生成一个模型 """ model_namespace = generate_model_name(model_name) with open(model_path, "r") as f: model_xml = f.read() rospy.wait_for_service('/gazebo/spawn_sdf_model') try: spawn_model_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) spawn_model_srv(model_namespace, model_xml, "", pose, "world") return model_namespace except rospy.ServiceException as e: print("生成模型失败: %s" % e) def delete_model(model_name): """ 从Gazebo中删除模型 """ model_namespace = generate_model_name(model_name) rospy.wait_for_service('/gazebo/delete_model') try: delete_model_srv = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) delete_model_srv(model_namespace) except rospy.ServiceException as e: print("删除模型失败: %s" % e) def move_model(model_name, velocity, direction, duration): """ 让模型运动一段时间 """ model_namespace = generate_model_name(model_name) model_pose_publisher = rospy.Publisher("/{}/pose".format(model_namespace), Pose, queue_size=10) rospy.init_node("move_model_node", anonymous=True) rate = rospy.Rate(10) # 发布频率为10Hz start_time = rospy.get_rostime().secs while (rospy.get_rostime().secs - start_time) < duration: pose = Pose() pose.position.x += velocity * math.cos(direction) pose.position.y += velocity * math.sin(direction) model_pose_publisher.publish(pose) rate.sleep() def main(): model_name = "aruco_cubo_hover" model_path = "/home/sjh/project/Tiago_ws/src/pal_gazebo_worlds/models/aruco_cube_hover/aruco_cube_hover.sdf" initial_pose = Pose() initial_pose.position.x = 0.0 # 设置模型初始x坐标 initial_pose.position.y = 0.0 # 设置模型初始y坐标 initial_pose.position.z = 0.0 # 设置模型初始z坐标 initial_pose.orientation.x = 0.0 # 设置模型初始姿态x initial_pose.orientation.y = 0.0 # 设置模型初始姿态y initial_pose.orientation.z = 0.0 # 设置模型初始姿态z initial_pose.orientation.w = 1.0 # 设置模型初始姿态w velocity = 0.1 # 设置模型速度 direction = math.pi / 4 # 设置模型运动方向，这里以弧度表示，假设为45度 duration = 10 # 设置运动持续时间（秒） # 在Gazebo中生成模型 model_namespace = spawn_model(model_name, model_path, initial_pose) # 让模型运动一段时间 move_model(model_name, velocity, direction, duration) # 删除模型 delete_model(model_name) if __name__ == "__main__": main()
