C++ 写一段订阅ros gazebo里移动小车的位置和速度的代码

Sure! Here's a sample code in C++ to subscribe to the and velocity of a mobile robot in ROS Gazebo:

#include <ros/ros.h>
#include <gazebo_msgs/ModelStates.h>

void modelStateCallback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
    // Find the index of the mobile robot within the ModelStates message
    int robotIndex = -1;
    for (int i = 0; i < msg->name.size(); i++)
    {
        if (msg->name[i] == "mobile_robot")
        {
            robotIndex = i;
            break;
        }
    }

    // If the mobile robot is found, print its position and velocity
    if (robotIndex != -1)
    {
        geometry_msgs::Pose robotPose = msg->pose[robotIndex];
        geometry_msgs::Twist robotTwist = msg->twist[robotIndex];

        ROS_INFO("Robot position: x = %f, y = %f, z = %f",
                 robotPose.position.x, robotPose.position.y, robotPose.position.z);

        ROS_INFO("Robot velocity: linear_x = %f, linear_y = %f, linear_z = %f",
                 robotTwist.linear.x, robotTwist.linear.y, robotTwist.linear.z);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mobile_robot_subscriber");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/gazebo/model_states", 1000, modelStateCallback);

    ros::spin();

    return 0;
}

In this code, we create a subscriber to the `/gazebo/model_states` topic, which provides the current state of all models in Gazebo. The `modelStateCallback` function is called whenever a new message is received on this topic. Inside the callback, we search for the mobile robot by name within the `ModelStates` message and then extract its position and velocity. Finally, we print the position and velocity using the `ROS_INFO` macro.

Please note that you need to replace `"mobile_robot"` with the actual name of your mobile robot model in Gazebo.

Remember to compile this code with the appropriate ROS package dependencies and run it within a ROS environment.