从0开始跑通小乌龟三个功能

提示文章写完后目录可以自动生成如何生成可参考右边的帮助文档文章目录前言一、pandas是什么二、使用步骤1.引入库2.读入数据总结前言学习完ROS的节点和话题通信我们接下来通过三个最基础的例子去更加深刻的学习话题通信和节点间的信号传输。第一个是通过键盘控制小乌龟的方向运动第二个是通过订阅话题去监控小乌龟的位置第三个是智能巡航给定一个目标自动导航到目标位置。一、按键控制小乌龟运动ROS2 的 Topic 可以理解成一个聊天频道群聊。Topic 的名字决定消息发送到哪个频道消息类型决定频道里大家使用什么格式交流。只有 Topic 名字相同且消息类型一致Publisher 和 Subscriber 才能成功通信。其中名字就相当于群聊的名字类型相当于要发送的内容举个例子你和一个外国人在一个群里面但是你们之间语言不通用所以你们无法通信。一个节点可以同时加入很多不同的频道也可以自己创建新的频道。这个小乌龟里面已经定义了发布者和订阅者的名字类型所以你想要进行控制或监听就必须按这样的格式去写。第一个工程Turtle_keyboard就是发布一个话题给小乌龟的订阅者去接收。小乌龟其实就相当于一个节点等开启时它的这些发布者和订阅者就开始不断发送消息。对于这个简单的控制我们将问题拆解首先要获取键盘数据、再将键盘数据转换成小乌龟可以识别的数据、最后再将转换好的数据进行发送。#include rclcpp/rclcpp.hpp #include geometry_msgs/msg/twist.hpp #include iostream /* std本质上命名空间里面存放来用于标准库的函数和类比如cout、cin、endl等 的作用就是去某个命名空间去找某个函数或类比如std::cout 也可以在类里使用当你定义了一个class后在类外实现某个函数时需要在类名前加:: 比如TurtleKeyboard::readKeyboard using namespace std;这个就是告诉编译器默认都去std空间下找 */ class TurtleKeyboard:public rclcpp::Node { public: TurtleKeyboard():Node(turtlekeyboard) { publisher_this-create_publishergeometry_msgs::msg::Twist(/turtle1/cmd_vel,10); RCLCPP_INFO(this-get_logger(),Turtle Keyboard Node Started); } void run();//声明一个函数把函数定义和函数实现分开因为以后类会非常大如果都写成函数内容会有几千行 private: //private 只限制谁可以调用不限制在哪里实现 char readKeyboard();//读取键盘指令 geometry_msgs::msg::Twist processKey(char key);//用于将键盘数据转换为Twist void publishVelocity(const geometry_msgs::msg::Twist msg);//用于发布Twist private: rclcpp::Publishergeometry_msgs::msg::Twist::SharedPtr publisher_; }; char TurtleKeyboard::readKeyboard()//键盘获取函数 { char key; std::cout请输入控制指令std::endl; std::cout W:前进 std::endl; std::cout A:左转 std::endl; std::cout D:右转 std::endl; std::cout S:停止 std::endl; std::cout Q:退出程序 std::endl; std::cout 输入; std::cinkey; return std::tolower(key);//兼容大小写 } geometry_msgs::msg::Twist TurtleKeyboard::processKey(char key) { geometry_msgs::msg::Twist msg{}; //不加{}就感觉在创建一个函数它的作用是初始化一个Twist对象用于存储速度控制 switch (key) { case w: msg.linear.y2.0; /* code */ break; case a: msg.linear.x-2.0; /* code */ break; case d: msg.linear.x2.0; /* code */ break; case s: msg.linear.y-2.0; break; default: break; } return msg;//因为要传的数据是geometry_msgs::msg::Twist } void TurtleKeyboard::run() { while (rclcpp::ok()) { char keyreadKeyboard();//这个会阻塞等待 if(keyq) { break; } auto msgprocessKey(key); publishVelocity(msg);//只发布来一次指令 } } /* 为什么发布来速度指令但是小乌龟却运动了一段距离呢 每次按键你只发布一次速度消息。在ROS中速度指令不是开关式的而是持续指令。 当小乌龟收到一次速度指令后它会按照这个速度移动但如果没有持续收到相同的速度指令它就会逐渐减速并停止。 */ void TurtleKeyboard::publishVelocity(const geometry_msgs::msg::Twist msg) { publisher_-publish(msg); } int main(int argc,char *argv[]) { rclcpp::init(argc,argv); auto nodestd::make_sharedTurtleKeyboard(); node-run();//外部调用这个函数运动 rclcpp::shutdown(); return 0; }完后运行这个工程的同时还需要运行小乌龟节点小乌龟节点运行方式如下然后就可以操作小乌龟进行运动。ros2 run turtlesim turtlesim_node二、订阅小乌龟运动状态这个项目就是实时监听小乌龟的坐标信息可以配合第一个项目进行联动当启动项目一时同时开启小乌龟监听节点即可获得小乌龟实时动态信息。/* 学会Subscribe订阅监控乌龟的位置 turtlesim │ ▼ /turtle1/pose │ ▼ Subscriber │ ▼ 打印当前位置 x 5.23 y 3.84 theta 1.52 linear_velocity 2.0 angular_velocity 0 */ #include rclcpp/rclcpp.hpp #include turtlesim/msg/pose.hpp ///turtle1/pose 这个话题发布的数据类型就是 turtlesim::msg::Pose //Subscriber 必须知道接收到的数据长什么样所以需要包含 turtlesim/msg/pose.hpp。 //geometry_msgs::msg::Twist 是 /turtle1/cmd_vel 使用的数据类型用来表示速度控制不适用于订阅位置话题。 #include iostream class TurtlePoseSubscriber:public rclcpp::Node { private: void pose_callback(const turtlesim::msg::Pose::SharedPtr msg)//msg是指针用于指向Pose对象的智能指针 { //对象用.指针用- RCLCPP_INFO(this-get_logger(), x%.2f y%.2f theta%.2f, msg-x, msg-y, msg-theta); } private: //定义一个订阅者用于订阅/turtle1/pose话题 rclcpp::Subscriptionturtlesim::msg::Pose::SharedPtr subscription_; public: //主要将构造函数初始化成员变量初始化在构造函数中 TurtlePoseSubscriber():Node(turtlePoseSubscriber) { //如果不赋值对象很快就会因为引用计数归零而销毁 subscription_this-create_subscriptionturtlesim::msg::Pose(/turtle1/pose,10, std::bind(TurtlePoseSubscriber::pose_callback,this,std::placeholders::_1)); //第三个参数需要的是一个可以被调用的函数 //std::placeholders::_1这个意思就是第一个参数以后再给回调函数。 } }; int main(int argc,char *argv[]) { rclcpp::init(argc,argv); auto nodestd::make_sharedTurtlePoseSubscriber();//创建一个节点用于订阅/turtle1/pose话题 rclcpp::spin(node); rclcpp::shutdown(); return 0; }这里要注意的知识点是订阅者和回调函数。订阅者的机制是当 ROS2 executor 发现 subscription 收到消息 → 就调用回调函数。流程如下publisher → DDS/中间层 → subscription队列 → executor → callback。回调函数的作用是打印输出位置信息。std::bind(TurtlePoseSubscriber::pose_callback, this, std::placeholders::_1)三、智能导航自动巡航到目标位置这个项目是对前面两个项目的综合运用将给小乌龟发送位置和订阅位置信息相融合通过反馈的位置信息不断修正自己的位置从而完成到达指定位置点的功能。/* 智能导航给定一个目标自动导航到目标位置。 */ #include rclcpp/rclcpp.hpp #include geometry_msgs/msg/twist.hpp//速度消息线速度和角速度 #include turtlesim/msg/pose.hpp//位姿消息 #include cmath class Turtle_navigation:public rclcpp::Node { private: void targetnode(const turtlesim::msg::Pose::SharedPtr pose) { geometry_msgs::msg::Twist message;//用于速度的接受后面给发布者去发布 //记录当前位置 double current_xpose-x; double current_ypose-y; double K{0.5};//控制系数 RCLCPP_INFO(this-get_logger(),current_x:%f,current_y:%f,current_x,current_y); //计算目标位置与当前位置的差值 double distancestd::sqrt((target_x-current_x)*(target_x-current_x)(target_y-current_y)*(target_y-current_y)); double anglestd::atan2(target_y-current_y,target_x-current_x)-pose-theta; //前面的先计算形成的三角形夹角后面的是当前和x轴的夹角 RCLCPP_INFO(this-get_logger(),distance:%f,angle:%f,distance,angle); //控制策略 if(distance0.5) { if(std::fabs(angle)0.5) { //向目标位置旋转再加速运动 message.angular.zK*angle; }else { //向目标位置加速运动 message.linear.xK*distance; } } publisher_-publish(message); } private: rclcpp::Publishergeometry_msgs::msg::Twist::SharedPtr publisher_; rclcpp::Subscriptionturtlesim::msg::Pose::SharedPtr subscription_; double target_x{3.0}; double target_y{3.0}; public:Turtle_navigation():Node(turtle_navigation) { publisher_this-create_publishergeometry_msgs::msg::Twist(/turtle1/cmd_vel,10); subscription_this-create_subscriptionturtlesim::msg::Pose(/turtle1/pose,10, std::bind(Turtle_navigation::targetnode,this,std::placeholders::_1)); } }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto nodestd::make_sharedTurtle_navigation(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }主要难点在于如何写好回调函数通过不断对比距离的差距采用的是先旋转方向再发送速度使其运动当足够靠近点的时候减小速度逐渐趋近于标定点。总结这三个工程主要是对Topic话题发布的订阅的应用。对于熟练掌握这部分知识点为后续的学习打下坚实的基础。