turtlesim小乌龟画最大正方形 c
时间: 2025-01-10 10:11:17 浏览: 2
使用 ROS Turtlesim 绘制最大正方形
为了实现这一目标,在 C 语言中操作 ROS 需要依赖于 roscpp
库而不是标准的 Python 接口。下面是一个完整的解决方案来让 Turtlesim 中的小乌龟绘制尽可能大的正方形。
初始化节点并发布消息
首先,创建一个新的 ROS 节点用于控制海龟模拟器中的移动命令:
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "draw_square_node");
ros::NodeHandle nh;
// 创建一个向/cmd_vel话题发布的publisher对象
ros::Publisher velocity_publisher = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
geometry_msgs::Twist vel_msg;
}
定义运动函数
定义两个辅助函数分别负责直线前进和转弯动作:
void moveStraight(double speed, double distance){
double t0 = ros::Time::now().toSec();
double current_distance = 0.0;
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = abs(speed);
while(current_distance < distance){
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_distance = speed * (t1-t0);
ros::spinOnce();
}
}
void rotate(double angular_speed, double angle){
double relative_angle = angle*PI/180;
double t0 = ros::Time::now().toSec();
double current_angle = 0.0;
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = abs(angular_speed);
while(current_angle < relative_angle){
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_angle = angular_speed*(t1-t0);
ros::spinOnce();
}
vel_msg.angular.z = 0;
velocity_publisher.publish(vel_msg);
}
主循环逻辑
最后设置主程序部分完成四个边角的动作序列:
if(__name__ == "__main__"){
try{
// 设置线速度与角度参数
double speed = 2.0; // 单位:m/s
double goal_distance = 4.0; // 边长设定为屏幕内可容纳的最大长度
double angular_speed = 1.57; // PI/2 radian per second
double goal_angle = 90; // Turn by 90 degrees after each side
for(int i=0;i<4;++i){ // Repeat four times to form a square
moveStraight(speed,goal_distance);
rotate(angular_speed,goal_angle);
}
ros::shutdown();
} catch(const std::exception& e){
ROS_ERROR("Exception caught: %s",e.what());
}
}
此代码片段展示了如何利用 ROS 的 C++ API 来指挥 Turtlesim 中的对象按照指定路径行动[^1]。
阅读全文