ROS学习笔记之小乌龟跟随

ROS学习笔记之小乌龟跟随

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

大体实现流程：

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

1. 启动 turtlesim 下的 turtlesim_node 节点和 turtle_teleop_key 节点。
2. 编写 .cpp 文件生成新的小乌龟。
3. 编写 .cpp 文件订阅分别两只小乌龟的坐标信息，并将坐标信息转换成相对于世界坐标系的坐标，并发布。
4. 编写 .cpp 文件订阅两只小乌龟相对于世界坐标系的坐标，将第一只小乌龟的坐标转换到第二只小乌龟的坐标系下，根据获取到的坐标信息编写跟随逻辑。
5. 配置相关的 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. 设置编码格式,初始化,创建nodehandle
        3. 创建 serviceClent
        4. 等待服务启动
        5. 设置新乌龟的参数,并创建新乌龟
 */


int main(int argc,char * argv[])
{
    // 2. 设置编码格式,初始化,创建nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"newTurtle");
    ros::NodeHandle nh;

    // 3. 创建 serviceClent
    ros::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;
}

其中的几个要点：

1. 在编写该文件之前，需要首先启动 turtlesim_node 节点，该节点启动之后能够产生新的小乌龟的 “/spawn” 的 service 才会启动。
2. 利用 client 产生新的小乌龟之前，需要对小乌龟的各个参数进行初始化。
3. 编写完成之后将该 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. 设置编码格式,初始化,创建 nodehandle
        3. 创建订阅者对象,订阅/turtle1/pose
        4. 利用回调函数处理订阅到的消息,将位姿信息转换成坐标相对关系,并发布
        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. 设置编码格式,初始化,创建 nodehandle
    setlocale(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/pose
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,dopose);
    // 4. 利用回调函数处理订阅到的消息,将位姿信息转换成坐标相对关系,并发布
    // 5. spin()
    ros::spin();
    return 0;
}

其中几个要点：

1. 要实现订阅坐标信息并将转换后的消息发送出去，就必须要有一个接收者和一个发送者。接收者通过 “turtle_name + “/pose” ” 话题接收 turtlesim::Poes 类型的消息，其中 turtle_name 就是 launch 文件中设置的 name ；发送者通过 /tf 话题，利用 sendTransform 函数发送 geometry_msgs::TransformStamped 类型的消息。
2. 利用 rostopic list 和 rosservice list 命令可查看当前存在的话题和服务，编写 .cpp 文件时会用到。
3. 编写完成之后，配置相应的 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. 设置编码格式，初始化，创建nodehandle
        3. 创建监听者对象
            3.1. 创建一个buffer
            3.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;
}

其中几个要点：

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