2021 Nqq

ROS 坐标变换实操(C++版本)(已整理)

文章目录

背景
一、建立launch文件 test.launch
二、通过服务通信生成一个新的乌龟节点
- 如何控制第2个乌龟运动
三、发布方(发布两只乌龟的坐标信息)
- 1、建立launch文件
- 2、test02_pub_turtle.cpp
三、订阅方(解析坐标信息并生成速度信息)
- 1、将 turtle1 转换成相对 turtle2 的坐标
- 2、建立launch文件
- 3、计算线速度和角速度并发布 test03_control_turtle2

背景

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

一、建立launch文件 test.launch

<launch>
    <!-- 1. 启动乌龟GUI节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
    <!-- 2. 生成新的乌龟节点 -->
    <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>

</launch>

二、通过服务通信生成一个新的乌龟节点

test01_new_turtle.cpp

#include"ros/ros.h"
#include"turtlesim/Spawn.h"// 消息类型

/*
    需求：向服务器发送请求，生成一个新的乌龟
        话题：/spawn
        消息：turtlesim/Spawn
    
    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建客户端对象
    5.组织数据并发送
    6.处理响应
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    // 2.初始化ROS节点
    ros::init(argc,argv,"service_call");

    // 3.创建节点句柄
    ros::NodeHandle nh;

    // 4.创建客户端对象
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn"); // /spawn是话题名称

    // 5.组织数据并发送
    // 5.1 组织请求数据
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";

    // 5.2 发送请求
    //判断服务器状态
    // ros::service::waitForService("/spawn");
    client.waitForExistence();
    bool flag = client.call(spawn);//flag 接受响应状态，响应结果也会被设置进spawn对象
    
    // 6.处理响应
    if(flag) // 响应成功
    {
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("请求失败!!!");
    }

    return 0;
}

终端输入
source ./devel/setup.bash
roslaunch tf04_test test.launch (启动launch文件生成一个新的乌龟)

在新的窗口输入
rostopic list

/rosout
/rosout_agg
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose

如何控制第2个乌龟运动

rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist （最后用tab补齐）

“linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0”

三、发布方(发布两只乌龟的坐标信息)

1、建立launch文件

<launch>
    <!-- 1. 启动乌龟GUI节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />

    <!-- 2. 生成新的乌龟节点 -->
    <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>

    <!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 -->
    <!-- 
        基本实现思路:
            1. 节点只编写一个
            2. 这个节点需要启动两次
            3. 节点启动时动态传参: 第1次启动传递turtle1，第2次传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"/>
    <!-- 由type类型确定和test02_pub_turtle.cpp建立联系 -->
</launch>

2、test02_pub_turtle.cpp

argc中的c指count，代表参数数量
*argv中的v是vector，代表参数数组
argv数组内容存放：程序名字参数A 参数B …
程序名字也算一个入口参数，以后使用参数的时候一定要从第argv下标1开始取才行

roslaunch tf04_test test.launch

因为有两个节点，所以该函数会启动两次

#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;

// 发布turtle1相对于世界坐标系的位置
// 给传进回调函数的数据定义范型 头文件2
void doPose(const turtlesim::Pose::ConstPtr& pose){
    // 获取位姿信息，转换成坐标系相对关系(核心)，并发布
    // a. 创建发布对象 头文件3
    static tf2_ros::TransformBroadcaster pub;

    // b. 组织被发布的数据 头文件4
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world"; // 相对于世界坐标系
    ts.header.stamp = ros::Time::now();
    // 关键点2: 动态传入
    // 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; // z一直是零（2D）
    // 坐标系四元数 头文件5
    /*
        位姿信息中没有四元数，但是有偏航角度，又已知乌龟时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();

    // c. 发布
    pub.sendTransform(ts);

}

int main(int argc, char *argv[])
{
    // 2. 设置编码，初始化， NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_turtle");
    ros::NodeHandle nh;

    /*
        解析launch文件，通过args传入的参数
    */
    if (argc != 2)// 成立时应该是两个
    {
        ROS_INFO("请传入一个参数");
        return 1;
    }else{
        turtle_name = argv[1];// argv[0]表示程序名称，argv[1]表示第一个参数就是 turtle1或者turtle2
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 3. 创建订阅对象，订阅/turtle1/pose
    // 关键点1: 订阅的话题名称，turtle1 或 turtle2 动态传入
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose); 

    // 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
    // 5. spin()
    ros::spin();
    return 0;
}

三、订阅方(解析坐标信息并生成速度信息)

1、将 turtle1 转换成相对 turtle2 的坐标

2、建立launch文件

<launch>
    <!-- 1. 启动乌龟GUI节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />

    <!-- 2. 生成新的乌龟节点 -->
    <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>

    <!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 -->
    <!-- 
        基本实现思路:
            1. 节点只编写一个
            2. 这个节点需要启动两次
            3. 节点启动时动态传参: 第1次启动传递turtle1，第2次传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"/>
    <!-- 由type类型确定和test02_pub_turtle.cpp建立联系 -->

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

</launch>

订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息；，。