#include "ros/ros.h" #include <geometry_msgs/Twist.h> int main(int argc, char *argv[]) { double PI = 3.141592653589793653589793; ros::init(argc, argv, "heart_shape"); ros::NodeHandle n; ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10); geometry_msgs::Twist vel_cmd; ros::Rate loopRate(2); int count = 0; int i = 0; while (ros::ok()) { geometry_msgs::Twist msg; msg.linear.x = 1; msg.angular.z = 1; i++; if (i ==9 ) { msg.linear.x = 0; msg.angular.z = 2*PI; } if (i == 16) { ; msg.linear.x = 1.0; msg.angular.z = 2; } if (i >= 17) { ; msg.linear.x = 1; msg.angular.z = 0; i++; if (i >= 27) { msg.linear.x = 0.15; msg.linear.y = 1; i++; if (i >= 43) { msg.linear.x = 0; msg.linear.y = 0; } } } vel_pub.publish(msg); ROS_INFO_STREAM("Sending random velocity command: " << "linear = " << msg.linear.x << " angular = " << msg.angular.z); loopRate.sleep(); } return 0; }以此为基础修改在surtlesim中画十字架

可以尝试这样修改代码：

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>

int main(int argc, char *argv[]) {
    double PI = 3.141592653589793653589793;

    // 初始化ROS节点
    ros::init(argc, argv, "cross_shape");
    ros::NodeHandle n;
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 设置控制速率
    ros::Rate loopRate(2);

    // 控制循环计数器
    int count = 0;
    int i = 0;

    // 控制命令消息
    geometry_msgs::Twist msg;

    while (ros::ok()) {
        // 发送控制命令
        vel_pub.publish(msg);

        ROS_INFO_STREAM("Sending random velocity command: "
                        << "linear = " << msg.linear.x
                        << " angular = " << msg.angular.z);

        // 等待一段时间
        loopRate.sleep();

        // 更新控制命令
        i++;
        if (i == 9) {
            // 绘制上部横线
            msg.linear.x = 1;
            msg.angular.z = 0;
        } else if (i == 11) {
            // 暂停一段时间
            msg.linear.x = 0;
            msg.angular.z = 0;
        } else if (i == 12) {
            // 绘制中间竖线
            msg.linear.x = 1;
            msg.angular.z = -PI / 2;
        } else if (i == 14) {
            // 暂停一段时间
            msg.linear.x = 0;
            msg.angular.z = 0;
        } else if (i == 15) {
            // 绘制下部横线
            msg.linear.x = 1;
            msg.angular.z = 0;
        } else if (i == 17) {
            // 暂停一段时间
            msg.linear.x = 0;
            msg.angular.z = 0;
        } else if (i == 18) {
            // 绘制中间竖线
            msg.linear.x = 1;
            msg.angular.z = -PI / 2;
        } else if (i == 20) {
            // 暂停一段时间
            msg.linear.x = 0;
            msg.angular.z = 0;
        } else if (i >= 21) {
            // 完成绘制，等待退出
            msg.linear.x = 0;
            msg.angular.z = 0;
        }
    }

    return 0;
}

这个代码会在turtlesim中绘制一个十字架形状。其中，每次更新控制命令时会根据计数器的值来决定应该绘制哪一条线段，同时也会在绘制完一条线段后暂停一段时间，等待绘制下一条线段。最后，当绘制完整个图形后，程序会等待一段时间后退出。