下面是一个使用ROS1和C++编程实现turtlesim中小海龟完成四个形状,并按原轨迹返回的示例代码:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <cmath>
const double PI = 3.14159265359;
void moveStraight(ros::Publisher& pub, double distance)
{
geometry_msgs::Twist msg;
msg.linear.x = 0.3;
double currentDistance = 0;
ros::Rate rate(10);
while (currentDistance < distance)
{
pub.publish(msg);
rate.sleep();
currentDistance += std::abs(msg.linear.x / rate.expectedCycleTime().toSec());
}
// 停止运动
msg.linear.x = 0;
pub.publish(msg);
}
void rotateLeft(ros::Publisher& pub, double angle)
{
geometry_msgs::Twist msg;
msg.angular.z = 0.5;
double currentAngle = 0;
ros::Rate rate(10);
while (currentAngle < angle)
{
pub.publish(msg);
rate.sleep();
currentAngle += std::abs(msg.angular.z / rate.expectedCycleTime().toSec());
}
// 停止旋转
msg.angular.z = 0;
pub.publish(msg);
}
void rotateRight(ros::Publisher& pub, double angle)
{
geometry_msgs::Twist msg;
msg.angular.z = -0.5; // 右转为负角速度
double currentAngle = 0;
ros::Rate rate(10);
while (currentAngle > -angle)
{
pub.publish(msg);
rate.sleep();
currentAngle -= std::abs(msg.angular.z / rate.expectedCycleTime().toSec());
}
// 停止旋转
msg.angular.z = 0;
pub.publish(msg);
}
void drawCircle(ros::Publisher& pub, double radius)
{
double distance = 2 * PI * radius;
moveStraight(pub, distance);
rotateLeft(pub, PI);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "turtle_shapes");
ros::NodeHandle nh;
ros::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn spawnSrv;
spawnSrv.request.x = 5.0;
spawnSrv.request.y = 5.0;
spawnSrv.request.name = "turtle2";
if (spawnClient.call(spawnSrv))
ROS_INFO("Spawned turtle successfully!");
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
// 直行0.3m
moveStraight(pub, 0.3);
// 左转后再直行0.3m
rotateLeft(pub, PI/2);
moveStraight(pub, 0.3);
// 右转直行0.3m
rotateRight(pub, PI/2);
moveStraight(pub, 0.3);
// 画半径为0.2m的半圆
drawCircle(pub, 0.2);
// 按原轨迹返回,需要逆序执行之前的动作
// 返回:画半径为0.2m的半圆
rotateRight(pub, PI);
moveStraight(pub, 0.2);
// 返回:右转直行0.3m
rotateLeft(pub, PI/2);
moveStraight(pub, 0.3);
// 返回:左转后再直行0.3m
rotateRight(pub, PI/2);
moveStraight(pub, 0.3);
// 返回:直行0.3m
moveStraight(pub, 0.3);
ros::spin();
return 0;
}
请确保你已经正确安装并配置了ROS1环境,以及turtlesim包。此代码将在启动时生成一个名为”turtle2”的小海龟,并按照指定的顺序完成四个形状,然后再按原轨迹返回。
内容由零声教学AI助手提供,问题来源于学员提问