G在ROS2中实现类似“/gazebo/set_model_state”的功能

ROS2采用launch.py文件来加载gazebo环境

model文件里是引入环境模型和机器人模型，他们都是sdf文件

实现：步骤一

<plugin filename="libgazebo_ros_state.so" name="gazebo_ros_state">
   <!-- Set the ROS node name for the plugin -->
   <ros>
   <namespace>/gazebo</namespace>
   </ros>
</plugin>

把代码放在world标签下面，然后用服务操作模型的坐标

操作方式如下（注意标签reference_frame选择world）

ros2 service call /gazebo/set_entity_state gazebo_msgs/srv/SetEntityState "state: {name: turtlebot3_burger, pose: {position:{x: 0.0, y: 0.0, z: 0.1}}, reference_frame: world}" 

实现：步骤二（c++代码实现）

#include "rclcpp/rclcpp.hpp"
#include "gazebo_msgs/srv/set_entity_state.hpp"

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("gazebo_service_client");

    // 创建一个用于发送服务请求的客户端
    auto client = node->create_client<gazebo_msgs::srv::SetEntityState>("/gazebo/set_entity_state");

    // 循环发送服务请求
    int num=0;
    while (rclcpp::ok()) {
        num=num+1;
        num=num%10;
        if (client->wait_for_service(std::chrono::seconds(1))) {
            auto request = std::make_shared<gazebo_msgs::srv::SetEntityState::Request>();

            // 填充请求参数
            request->state.name = "turtlebot3_burger";
            request->state.pose.position.x = -(num+1);
            request->state.pose.position.y = -(num+1);
            request->state.pose.position.z = double(num%100);
            request->state.reference_frame = "world";

            // 发送服务请求（异步）
            client->async_send_request(request);

            // 可以在此添加其他操作，不需要等待响应

        } else {
            RCLCPP_ERROR(node->get_logger(), "Service not available");
        }

        // 延时一段时间后再次发送请求
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    rclcpp::shutdown();
    return 0;
}