自动追随跟踪

最新推荐文章于 2024-04-25 10:45:14 发布

三个刺客

最新推荐文章于 2024-04-25 10:45:14 发布

阅读量548

点赞数

分类专栏： ROS 文章标签：自动驾驶 linux

本文链接：https://blog.csdn.net/mxh3600/article/details/125334979

版权

ROS 专栏收录该内容

23 篇文章 12 订阅

订阅专栏

需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

启动乌龟显示节点
在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
编写两只乌龟发布坐标信息的节点
编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程:C++ 与 Python 实现流程一致

新建功能包，添加依赖
编写服务客户端，用于生成一只新的乌龟
编写发布方，发布两只乌龟的坐标信息
编写订阅方，订阅两只乌龟信息，生成速度信息并发布
运行

准备工作:

1.了解如何创建第二只乌龟，且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"
name: "turtle_flow"

键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度

1.创建功能包 tf04_test

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟) test01_new_turtle.cpp

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"service_call");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    client.waitForExistence();
    bool flag = client.call(spawn);
    if(flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫:%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("请求失败！！！");
    }
    return 0;
}

生成新的乌龟 test.launch

<launch>
      <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
      <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
      <node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" />
</launch>

键盘只能控制turtle1，原因是键盘控制代码里只是默认有控制turtle1的代码

3.发布方(发布两只乌龟的坐标信息) test02_pub_turtle.cpp

可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

该节点需要启动两次
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

#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创建订阅对象、订阅/turtle/pose
       4回调函数处理订阅的消息，将位姿信息转换成坐标相对关系并发布
       5spin()
*/

//声明变量接受传递的参数
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr& pose){
     //获取位姿信息，转换成坐标系相对关系，并发布
     //1创建TF发布对象
     static tf2_ros::TransformBroadcaster pub;
     //2组织被发布的数据
     geometry_msgs::TransformStamped ts;
     ts.header.frame_id = "world";
     ts.header.stamp = ros::Time::now();
     //ts.child_frame_id ="turtle1";
     ts.child_frame_id =turtle_name;
     //坐标系偏移量设置
     ts.transform.translation.x = pose->x;
     ts.transform.translation.y = pose->y;
     ts.transform.translation.z = 0;
     //坐标系四元数
     /*
      位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是2D，没有翻滚与俯仰角度，
      所以可以得出乌龟的欧拉角：0 0 theta
    */
     tf2::Quaternion qtn;
     qtn.setRPY(0,0,pose->theta);

     ts.transform.rotation.x = qtn.getX();
     ts.transform.rotation.y = qtn.getY();
     ts.transform.rotation.z = qtn.getZ();
     ts.transform.rotation.w = qtn.getW();
     //3发布
     pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    /*
           解析launch文件通过args传入的参数
   */
    if (argc != 2)
    {
        ROS_ERROR("请传入一个参数");
        return 1;
    }
    else{
        turtle_name = argv[1];
    }
    
    
    //3创建订阅对象、订阅/turtle1/pose
    //关键点1：订阅的话题名称，turtle1或turtle2动态传入
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);
    ros::spin();
    return 0;
}

//声明变量接受传递的参数
std::string turtle_name;

test03_control_turtle2.cpp

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
     需求1：换算出turtle1相对于turtle2的关系
     需求2：计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //A创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1计算son1与son2的相对关系
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            //ROS_INFO("son1相对于son2的信息:父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f) ",
                     // son1ToSon2.header.frame_id.c_str(),  //turtle2
                     // son1ToSon2.child_frame_id.c_str(),   //turtle1
                     // son1ToSon2.transform.translation.x,
                     // son1ToSon2.transform.translation.y,
                     // son1ToSon2.transform.translation.z
                     // );

             //B根据相对计算并组织速度消息
             geometry_msgs::Twist twist;
             /*
                组织速度，只需要设置线速度的x与角速度的z
                 x = 系数 * 开方(y^2 + x^2)
                z = 系数 * 反正切(对边，邻边)
             */
            twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
            twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);
             //C发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

运行 test.launch

<launch>
      <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
      <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
      <node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" />

      <!--3需要启动两个乌龟相对世界的坐标关系的发布 -->
      <!--
          基本实现思路：
               1.节点只编写一个
               2.这个节点需要启动两次
               3.节点启动时动态传参：第一次启动传递turtle1 第二次启动传递turtle2
      -->
      <node pkg="tf04_test" type="test02_pub_turtle" name="pub1" args="turtle1" output="screen" />
      <node pkg="tf04_test" type="test02_pub_turtle" name="pub2" args="turtle2" output="screen" />

      <!--4.需要订阅turtle1与turtle2相对于世界坐标系的坐标消息，
        并转换成turtle1相对于turtle2的坐标关系 
        再生成速度消息
      -->
      <node pkg="tf04_test" type="test03_control_turtle2" name="control" output="screen" />
      
</launch>

roslaunch tf04_test test.launch