ROS学习笔记之小乌龟跟随

说明：整个案例是跟着赵虚左老师的视频和文档资料学习的，特此感谢赵虚左老师和Autolabor官方
文档地址
视频地址
学习案例之前的预备知识：TF坐标变换

大体实现流程：

需求分析：一只小乌龟跟随另一只小乌龟运动

启动 turtlesim 下的 turtlesim_node 节点和 turtle_teleop_key 节点。
编写 .cpp 文件生成新的小乌龟。
编写 .cpp 文件订阅分别两只小乌龟的坐标信息，并将坐标信息转换成相对于世界坐标系的坐标，并发布。
编写 .cpp 文件订阅两只小乌龟相对于世界坐标系的坐标，将第一只小乌龟的坐标转换到第二只小乌龟的坐标系下，根据获取到的坐标信息编写跟随逻辑。
配置相关的 CMakeLists.txt 文件，并编写相应的 launch 文件。
其中主要任务为2、3、4三个 .cpp 文件的编写

一. 创建新的小乌龟

创建该 .cpp 文件之前，需要首先启动 turtlesim_node 节点，可以新建 launch 文件快速启动，例如在 launch 文件中添加如下代码：

<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"   />

下面是 .cpp 文件的具体内容：

#include "ros/ros.h"
#include "turtlesim/Spawn.h"/* 需求: 创建一只新的小乌龟实现流程:1. 包含头文件2. 设置编码格式,初始化,创建nodehandle3. 创建 serviceClent4. 等待服务启动5. 设置新乌龟的参数,并创建新乌龟*/int main(int argc,char * argv[])
{// 2. 设置编码格式,初始化,创建nodehandlesetlocale(LC_ALL,"");ros::init(argc,argv,"newTurtle");ros::NodeHandle nh;// 3. 创建 serviceClentros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");// 4. 等待服务启动ros::service::waitForService("/spawn");// 5. 设置新乌龟的参数,并创建新乌龟turtlesim::Spawn t_spawn;t_spawn.request.x = 1.0;t_spawn.request.y = 1.0;t_spawn.request.theta = 1.57;t_spawn.request.name = "turtle2";bool flag = client.call(t_spawn);//创建新的乌龟//处理响应结果if(flag){ROS_INFO("新乌龟的名字为：%s",t_spawn.response.name.c_str());}else{ROS_INFO("小乌龟生成失败！");}return 0;
}

其中的几个要点：

在编写该文件之前，需要首先启动 turtlesim_node 节点，该节点启动之后能够产生新的小乌龟的 “/spawn” 的 service 才会启动。
利用 client 产生新的小乌龟之前，需要对小乌龟的各个参数进行初始化。
编写完成之后将该 node 添加到 launch 文件中。

二. 订阅两只小乌龟的坐标信息，转换成世界坐标系并发送

创建该文件之前，我们首先明确一点，即我们需要作两次如下动作：订阅当前的坐标信息、将当前坐标信息转换到世界坐标系下、发布世界坐标系下的坐标。我们可以写两个几乎一模一样的文件，改一改其中的参数，但是这样我们的代码就太 low 了，两个文件最大区别就是参数的不同，因此我们只需要改一下 launch 文件里的参数，然后启动两遍 node 就可以了。launch 文件中可加入如下代码：

<node pkg = "ros_tf2_turtle_flow" type = "turtle_flow" name = "turtle1_info" args = "turtle1" output = "screen" />
<node pkg = "ros_tf2_turtle_flow" type = "turtle_flow" name = "turtle2_info" args = "turtle2" output = "screen" />

下面是 .cpp 文件的具体内容：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"/*需求:订阅乌龟的位姿信息,然后转换成相对于窗体的坐标关系,并发布准备:话题:/turtle1/pose消息:turtlesim/Pose实现流程:1. 包含头文件2. 设置编码格式,初始化,创建 nodehandle3. 创建订阅者对象,订阅/turtle1/pose4. 利用回调函数处理订阅到的消息,将位姿信息转换成坐标相对关系,并发布5. spin()*/std::string turtle_name;//回调函数
void dopose(const turtlesim::Pose::ConstPtr & Pose)
{/*实现流程:1. 创建订阅者对象2. 处理消息3. 发布*///创建发布者对象static tf2_ros::TransformBroadcaster ts;//创建的是动态发布者//创建消息载体geometry_msgs::TransformStamped ts_msgs;//四元数换算tf2::Quaternion qt;qt.setRPY(0,0,Pose->theta);//信息的基本信息设定ts_msgs.header.frame_id = "world";ts_msgs.header.stamp = ros::Time::now();ts_msgs.child_frame_id = turtle_name;//坐标轴方向上的转换ts_msgs.transform.translation.x = Pose->x;ts_msgs.transform.translation.y = Pose->y;ts_msgs.transform.translation.z = 0;//俯仰,偏航,翻滚三个旋转轴的变换ts_msgs.transform.rotation.x = qt.getX();ts_msgs.transform.rotation.y = qt.getY();ts_msgs.transform.rotation.z = qt.getZ();ts_msgs.transform.rotation.w = qt.getW();//发布数据ts.sendTransform(ts_msgs);//默认使用的话题为 /tf  表示动态话题}int main(int argc,char * argv[])
{// 2. 设置编码格式,初始化,创建 nodehandlesetlocale(LC_ALL,"");ros::init(argc,argv,"turtle_flow");ros::NodeHandle nh;if (argc != 2){ROS_ERROR("请输入正确的参数");}else{turtle_name = argv[1];ROS_INFO("%s 坐标已发送！",turtle_name.c_str());}// 3. 创建订阅者对象,订阅/turtle1/poseros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,dopose);// 4. 利用回调函数处理订阅到的消息,将位姿信息转换成坐标相对关系,并发布// 5. spin()ros::spin();return 0;
}

其中几个要点：

要实现订阅坐标信息并将转换后的消息发送出去，就必须要有一个接收者和一个发送者。接收者通过 “turtle_name + “/pose” ” 话题接收 turtlesim::Poes 类型的消息，其中 turtle_name 就是 launch 文件中设置的 name ；发送者通过 /tf 话题，利用 sendTransform 函数发送 geometry_msgs::TransformStamped 类型的消息。
利用 rostopic list 和 rosservice list 命令可查看当前存在的话题和服务，编写 .cpp 文件时会用到。
编写完成之后，配置相应的 CMakeLists.txt 文件和launch 文件。

三. 订阅世界坐标系下的坐标，并转换成两只乌龟的相对坐标

这里的两只乌龟的相对坐标指的是将第一只小乌龟的坐标转换到第二只小乌龟的坐标系下
下面为具体代码：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/Twist.h"/*需求： 订阅两个小乌龟在世界坐标系下的坐标，并将turtle1的坐标转换到turtle2坐标系下实现流程：1. 包含头文件2. 设置编码格式，初始化，创建nodehandle3. 创建监听者对象3.1. 创建一个buffer3.2. 创建监听者对象4. 编写跟踪逻辑5. spin();*/int main(int argc,char * argv[])
{setlocale(LC_ALL,"");ros::init(argc,argv,"flow_sub");ros::NodeHandle nh;tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);geometry_msgs::Twist twist;ros::Rate rate(10);while (ros::ok()){/* code */try{/* code */geometry_msgs::TransformStamped msgs_trans = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));    twist.linear.x = 0.5 * sqrt(pow(msgs_trans.transform.translation.x,2) + pow(msgs_trans.transform.translation.y,2));twist.angular.z = 4 * atan2(msgs_trans.transform.translation.y,msgs_trans.transform.translation.x);pub.publish(twist);}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("程序异常：%s",e.what());}rate.sleep();ros::spinOnce();}return 0;
}

其中几个要点：

我们需要接收到两只小乌龟在世界坐标系下的坐标，就需要一个接收者 listener ，接收到坐标信息之后进行转换，将第一只小乌龟的坐标信息转换到第二只小乌龟的坐标系下。转换完成后，我们需要根据第一只小乌龟在第二只小乌龟坐标系下的坐标位置去编写第二只小乌龟的跟踪逻辑，编写完成之后，我们需要一个发送者 pub 通过 /turtle2/cmd_vel 话题发送 geometry_msgs::Twist 类型的数据，然后第二只小乌龟就会接收到这些消息完成跟踪。
比较重要的是在搞清楚大体的流程之后，需要明白各个对象通信的具体的话题和消息载体是什么。
跟踪逻辑那里是复制 Autolabor 官网的代码，也可以自己写。
这些函数我就是硬记的，如果有什么总结归纳的方法的话，欢迎补充。

ROS学习笔记之小乌龟跟随相关推荐

ROS学习笔记-多机器人通信（1）-实现两台机器通信
ROS是一个分布式的计算环境.一个正在运行的ROS可以在多个机器人之间分布成几十甚至上百个节点.取决于系统的配置方式,任何节点可能需要随时与任何其他节点进行通信,为实现使用同一个master控制多台机 ...
ROS 学习笔记（三）：自定义服务数据srv+server+client 示例运行
ROS 学习笔记(三):自定义服务数据srv+Server+Client 示例运行一.自定义服务数据: 1.向功能包添加自定义服务文件(AddTwoInts.srv) cd ~/catkin_ws/ ...
ROS 学习笔记（二）：自定义消息msg+Publisher+Subscriber 示例运行
ROS 学习笔记(二):自定义消息msg+Publisher+Subscriber 示例运行一.自定义消息: 1.新建msg文件夹,创建定义Person.msg 文件 mkdir -p ~/catk ...
ROS 学习笔记（一）：工作空间+功能包创建
ROS 学习笔记(一):工作空间+功能包创建一.创建工作空间(catkin_make编译): 1.创建工作空间 catkin_ws 创建空间.初始化(建立一个文件夹) cd ~ mkdir -p ~ ...
ROS学习笔记（八）： ROS通信架构
ROS学习笔记(八): ROS通信架构文章目录 01 Node & Master 1.1 Node 1.2 Master 1.3 启动master和node 1.4 rosrun和rosno ...
ROS学习笔记基础2（基础知识和ROS架构）
ROS学习笔记1(基础知识和ROS架构) 文章目录 ROS学习笔记1(基础知识和ROS架构) 1. 什么是ROS 2. ROS和其他机器人平台有什么不同 3. ROS架构组成 3.1 文件系统级别 3 ...
ROS学习笔记十二：使用roswtf
ROS学习笔记十二:使用roswtf 在使用ROS过程中,roswtf工具可以为我们提供ROS系统是否正常工作的检查作用. 注意:在进行下列操作之前,请确保roscore没有运行. 检查ROS是否安装 ...
ROS学习笔记十一：ROS中数据的记录与重放
ROS学习笔记十一:ROS中数据的记录与重放本节主要介绍如何记录一个正在运行的ROS系统中的数据,然后在一个运行的系统中根据记录文件重新产生和记录时类似的运动情况.本例子还是以小海龟例程为例. 记录 ...
ROS学习笔记十：用C++编写一个简单的服务和客户端
ROS学习笔记十:用C++编写一个简单的服务和客户端这一节主要介绍如何使用C++编写一个简单的服务和客户端节点. 编写服务节点由于在前面的练习中,已经向beginner_tutorials软件包中 ...

ROS学习笔记之小乌龟跟随

ROS学习笔记之小乌龟跟随

大体实现流程：

一. 创建新的小乌龟

二. 订阅两只小乌龟的坐标信息，转换成世界坐标系并发送

三. 订阅世界坐标系下的坐标，并转换成两只乌龟的相对坐标

ROS学习笔记之小乌龟跟随相关推荐

最新文章

热门文章