#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; } 以此为基础修改个在turtlesim画五角星
时间: 2023-12-12 19:02:53 浏览: 160
好的,可以参考以下代码实现在turtlesim中画五角星:
```
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
int main(int argc, char *argv[]) {
double PI = 3.141592653589793653589793;
ros::init(argc, argv, "star_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 = PI/5; // 五角星的内角为 180 * (5 - 2) / 5 = 108 度,对应的弧度为 PI/5
i++;
if (i == 4) {
msg.linear.x = 0;
msg.angular.z = -PI/2;
} else if (i == 6) {
msg.linear.x = 1.0;
msg.angular.z = -3*PI/4;
} else if (i == 8) {
msg.linear.x = 1;
msg.angular.z = -PI/2;
} else if (i == 10) {
msg.linear.x = 1.0;
msg.angular.z = -PI/4;
} else if (i == 12) {
msg.linear.x = 1;
msg.angular.z = PI/2;
} else if (i >= 13) {
msg.linear.x = 1;
msg.angular.z = 0;
i++;
if (i >= 23) {
msg.linear.x = 0.15;
msg.linear.y = 1;
i++;
if (i >= 39) {
msg.linear.x = 0;
msg.linear.y = 0;
}
}
}
vel_pub.publish(msg);
ROS_INFO_STREAM("Sending velocity command: " << "linear = " << msg.linear.x << " angular = " << msg.angular.z);
loopRate.sleep();
}
return 0;
}
```
在这个代码中,我们在原来的基础上进行了如下修改:
1. 将节点名改为 "star_shape"。
2. 将五角星的内角计算出来,并赋值给 `msg.angular.z`。
3. 在循环中,根据需要修改 `msg.linear.x` 和 `msg.angular.z` 的值,以实现五角星的绘制。具体方式为:先让小海龟向前走,然后根据需要转动一定的角度,最后停止转动并继续向前走。
4. 在每次循环结束时,打印出当前发送的速度指令。
需要注意的是,五角星的绘制是通过在一定的角度上旋转小海龟实现的。因此在计算角度时需要使用弧度制而非角度制。同时,小海龟在旋转时,角度为正表示逆时针旋转,角度为负表示顺时针旋转。
阅读全文