gazebo环境动态初始化

用python代码动态放置墙面、障碍物、行人

import rospy
import math

from geometry_msgs.msg import Pose, Point, Quaternion

from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import DeleteModel, SpawnModel
from gazebo_ros.gazebo_interface import SetModelState

import tf


# 向GAZEBO世界放置模型
def spawn_sdf_model(name, path, x, y, z):
    initial_pose = Pose(position=Point(x=x, y=y, z=z))
    # 从文件加载模型
    with open(path, "r") as f:
        model_xml = f.read()
    # 调用Gazebo的SpawnModel服务
    spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
    resp_sdf = spawn_model(name, model_xml, "", initial_pose, "world")
    if resp_sdf.success:
        rospy.loginfo("model '{}' generate success".format(name))
    else:
        rospy.logerr("model '{}' generate fail".format(name))


# 删除GAZEBO模型
def del_model(modelName):
    del_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
    del_model_prox(str(modelName)) # Remove from Gazebo


# 修改GAZEBO模型位置
def set_gazebo_model_position(model_name, x, y, z, yaw):
    state_msg = ModelState()
    state_msg.model_name = model_name
    state_msg.pose.position.x = x
    state_msg.pose.position.y = y
    state_msg.pose.position.z = z
    state_msg.pose.orientation.x = 0
    state_msg.pose.orientation.y = 0
    state_msg.pose.orientation.z = math.sin(0.5 * yaw)
    state_msg.pose.orientation.w = math.cos(0.5 * yaw)
    set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
    resp = set_state(state_msg)


def set_obstacle(ref_point, obstacle_name, s_bias, l_bias):
    (_, _, yaw) = tf.transformations.euler_from_quaternion([0, 0, ref_point[2], ref_point[3]])
    x = ref_point[0]
    y = ref_point[1]
    x += l_bias * math.cos(yaw + 0.5 * math.pi)
    y += l_bias * math.sin(yaw + 0.5 * math.pi)
    x += s_bias * math.cos(yaw)
    y += s_bias * math.sin(yaw)
    set_gazebo_model_position(obstacle_name, x, y, 0.0, yaw)