#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; }修改为循环画无限符号
时间: 2023-12-12 22:03:02 浏览: 151
好的,根据您的需求,我修改了代码如下:
```
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
int main(int argc, char *argv[])
{
double PI = 3.141592653589793653589793;
ros::init(argc, argv, "infinity_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;
}
else if (i == 16)
{
msg.linear.x = 1.0;
msg.angular.z = 2;
}
else if (i >= 17 && i < 27)
{
msg.linear.x = 1;
msg.angular.z = 0;
}
else if (i >= 27 && i < 36)
{
msg.linear.x = 1.0;
msg.angular.z = -2;
}
else if (i == 36)
{
msg.linear.x = 0;
msg.angular.z = 2 * PI;
}
else if (i >= 37 && i < 47)
{
msg.linear.x = 1;
msg.angular.z = 0;
}
else if (i >= 47 && i < 56)
{
msg.linear.x = 1.0;
msg.angular.z = 2;
}
else if (i == 56)
{
msg.linear.x = 0;
msg.angular.z = 2 * PI;
}
else if (i >= 57 && i < 67)
{
msg.linear.x = 1;
msg.angular.z = 0;
}
else if (i >= 67 && i < 76)
{
msg.linear.x = 1.0;
msg.angular.z = -2;
}
else if (i == 76)
{
msg.linear.x = 0;
msg.angular.z = 2 * PI;
}
else if (i >= 77 && i < 87)
{
msg.linear.x = 1;
msg.angular.z = 0;
}
else if (i >= 87 && i < 96)
{
msg.linear.x = 1.0;
msg.angular.z = 2;
}
else if (i == 96)
{
i = 0;
}
vel_pub.publish(msg);
ROS_INFO_STREAM("Sending velocity command: "
<< "linear = " << msg.linear.x
<< " angular = " << msg.angular.z);
loopRate.sleep();
}
return 0;
}
```
这个代码可以画出无限符号,通过修改 `i` 的值来控制不同的运动状态。我在代码中添加了多个 `if-else` 条件语句,用于控制不同的运动状态。您可以根据需要自行修改这些参数来调整符号的形状和大小。
注意,在代码的最后,当 `i` 的值达到 96 时,我们将 `i` 的值重置为 0,从而使符号的运动可以无限循环。
阅读全文