【Gazebo】场景模型生成及生成坐标不正确问题

场景模型生成及生成坐标不正确问题

1. ROS Service

在ROS中，服务（Services）是一种允许节点之间进行双向通信的机制，允许节点请求特定功能并等待响应。服务允许节点发送请求并接收一个唯一的响应，通常用于执行某项任务或获取一些信息。
Request（请求）: 定义了客户端请求所需提供的数据类型。
Response（响应）: 定义了服务端响应所需提供的数据类型。

详情参考此博文

在ROS中，服务可以基于不同的通信机制和消息类型，比如：

rospy中的服务: 在Python中使用 rospy 库来创建和调用服务。
roscpp中的服务: 在C++中使用 roscpp 库来创建和调用服务。

创建服务: 定义 .srv 文件，并在代码中使用 rospy 或 roscpp 创建服务。
调用服务: 使用 rospy 或 roscpp 提供的方法调用服务。

2. Gazebo Service API

1. spawn_model 服务
   服务名称: /gazebo/spawn_sdf_model 或 /gazebo/spawn_urdf_model
   描述: 允许用户在Gazebo中生成一个SDF或URDF模型。可以指定模型名称、模型文件路径、命名空间、初始位姿等信息。
   参数

参数	作用	类型
model_name	模型名称	string
model_xml	SDF或URDF模型的XML内容	string
robot_namespace	机器人的命名空间	string
initial_pose	模型的初始位姿	geometry_msgs/Pose
reference_frame	模参考坐标系（通常为 “world”）	string

2. delete_model 服务
   服务名称: /gazebo/delete_model
   描述: 用于删除Gazebo中的指定模型。通过提供模型名称作为参数，可以从仿真环境中移除特定模型。

参数	作用	类型
model_name	模型名称	string

3. set_model_state 服务
   服务名称: /gazebo/set_model_state
   描述: 允许用户设置模型的状态，包括位置、姿态、线速度和角速度等。
4. get_model_state 服务
   服务名称: /gazebo/get_model_state
   描述: 用于获取特定模型的状态信息，包括位置、姿态、线速度和角速度等。
5. set_physics_properties 服务
   服务名称: /gazebo/set_physics_properties
   描述: 允许用户修改仿真环境的物理属性，如重力、时间步长、阻尼等。
6. get_physics_properties 服务
   服务名称: /gazebo/get_physics_properties
   描述: 用于获取当前仿真环境的物理属性信息，如重力、时间步长、阻尼等。
7. 消息类型
   ModelState: 用于设置和获取模型的状态信息，包括位置、姿态和速度等。
   ModelStates: 包含多个模型的状态信息，用于批量获取模型状态。
   LinkStates: 用于获取模型中链接的状态信息，包括链接的名称、位置、姿态和速度等。

3. Gazebo场景模型创建与删除demo

import rospy
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SpawnModel
from gazebo_msgs.srv import DeleteModel
from geometry_msgs.msg import Quaternion
from tf import transformations


def rpy_to_quaternion(roll, pitch, yaw):
    quaternion = transformations.quaternion_from_euler(roll, pitch, yaw)
    return Quaternion(*quaternion)


def delete_model(model_name):
    rospy.wait_for_service('/gazebo/delete_model')
    try:
        delete_model_prox = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
        delete_model_prox(model_name)
        rospy.loginfo("Model '{}' deleted.".format(model_name))
    except rospy.ServiceException as e:
        rospy.logerr("Model deletion failed: {}".format(e))


def insert_model(quaternion, initial_pose, model_name, model_folder):
    rospy.init_node('insert_object', log_level=rospy.INFO)

    model_path = '/home/moresweet/.gazebo/models/{}/model.sdf'.format(model_folder)

    with open(model_path, 'r') as f:
        sdff = f.read()

    initial_pose.orientation = quaternion

    rospy.wait_for_service('gazebo/spawn_sdf_model')
    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
    spawn_model_prox(model_name, sdff, "/", initial_pose, "world")

namespace默认为/

namespace = rospy.get_namespace()
rospy.loginfo("Current namespace: {}".format(namespace))

使用

roll = 0.0
pitch = 0.0
yaw = 1.57

quaternion = rpy_to_quaternion(roll, pitch, yaw)

initial_pose = Pose()
initial_pose.position.x = -4.314347
initial_pose.position.y = 2.300168
initial_pose.position.z = 0.038283
#
model_name = "sofa"
model_folder = "sofa_set_2"  # 替换为你的文件夹名称
insert_model(quaternion, initial_pose, model_name, model_folder)

4. 模型生成位置不正确问题参考方案

在笔者的场景中，不知是否是官方bug，当模型名和文件的模型名字一致时会出现位置不正确的情况
如下

pose = Pose()
    # sofa area
    quaternion = rpy_to_quaternion(0.0, 0.0, 1.57)
    pose.position.x = -4.314347
    pose.position.y = 2.300170
    pose.position.z = 0.038283
    insert_model(quaternion, pose, "sofa_set_2", "sofa_set_2")

实际位置应该是红色框的部分，而且无论参数如何变化，都是生成在那个位置
修改场景名字后，放置正常

pose = Pose()
    # sofa area
    quaternion = rpy_to_quaternion(0.0, 0.0, 1.57)
    pose.position.x = -4.314347
    pose.position.y = 2.300170
    pose.position.z = 0.038283
    insert_model(quaternion, pose, "sofa", "sofa_set_2")