ROS学习笔记3（坐标变换）

书接上会（静态坐标变换也在里面）

5.2 动态坐标变换

准备工作：获取乌龟的所需信息

在第一篇笔记里有详细记录，这里仅回忆步骤

启动节点

rosrun turtlesim turtlesim_node

获取信息

rostopic list
rostopic info /turtle1/pose
rosmsg info turtlesim/Pose

5.2.1 发布方

编码如下

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //组织被发布数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标系偏移量设置
    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();
   pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    ros::spin();
    return 0;
}

一定要记得静态！！！！！ (2024/12/29 早，有个憨批忘记静态调试了一个小时）

5.2.2 订阅方

订阅方和静态基本一致

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

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //创建buffer(缓存)
    tf2_ros::Buffer buffer;
    //创建监听对象(将订阅数据存入buffer中)
    tf2_ros::TransformListener listener(buffer);
    geometry_msgs::PointStamped ps;
    //位置基于laser对象
    ps.header.frame_id = "turtle1";
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    ros::Rate rate(1);
    // ros::Duration(2).sleep();
    while(ros::ok())
    {
        try
        {
             //将ps转化为相对于base_link的坐标对象
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"world");
        ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考坐标系:%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            ROS_INFO("异常消息：%s",e.what());
        }
        
       
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

唯一不同的地方

    ps.header.stamp = ros::Time(0.0);

这里没有将时间辍设置成now，因为发布方是不停发布的，有延时，将数据存入buffer也有延时。坐标点也有时间辍，转换的时候，会拿坐标点的时间辍和坐标关系的时间辍进行比对，当比对结果接近时会进行转换，而当时间差过大时，会判定为转换结果误差大，抛出异常。将时间辍设置为零值，则不会进行比对。

5.3 多坐标变换

5.3.1 发布方

可以直接使用launch文件代替命令行实现

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="3 0 0 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

5.3.2 订阅方

编码如下

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

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);
    //创建坐标点
    geometry_msgs::PointStamped psAtSon1;
    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(1);
    while (ros::ok())
    {
        try
        {
            //1.计算son1与son2的相对关系
            /*
                参数1：目标坐标系
                参数2：源坐标系
                参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1 相对于 son2 的信息：父极：%s, 子极：%s 偏移量(%.2f,%.2f,%.2f)",
                    son1ToSon2.header.frame_id.c_str(),
                    son1ToSon2.child_frame_id.c_str(),
                    son1ToSon2.transform.translation.x,
                    son1ToSon2.transform.translation.y,
                    son1ToSon2.transform.translation.z
                    );
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );
        }
        catch(const std::exception& e)
        {
            
            ROS_INFO("异常消息：%s",e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

实现效果

ros::Time(0) 取最近的两帧

 5.4 坐标关系查看

启动坐标系广播程序之后，运行如下命令:

rosrun tf2_tools view_frames.py

会产生如下日志

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

其实就是监听了坐标系信息并生成了PDF文件

调用命令查看文件

evince frames.pdf

也可以在目录里直接打开

5.5 坐标变换实操

5.5.1 新建功能包并生成乌龟

此处复用的是2.5.3的代码

中间增加了延时，防止节点运行时turtlesim_node没有启动

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

using namespace ros;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"new_turtle");
    NodeHandle nh;
    ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.x = 1;
    spawn.request.y = 1;
    spawn.request.theta = 1;
    spawn.request.name = "turtle2";
    Duration(0.2).sleep();
    client.call(spawn);
    bool flag = client.waitForExistence();
    if(flag)
    {
        ROS_INFO("启动成功");
    }
    else
    {
        ROS_WARN("启动失败");
    }
    return 0;
}

新建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="tf_test" type="new_turtlesim" name="turtle2" output="screen" />
</launch>

5.5.2 两只乌龟的坐标发布

此处复用5.2.1的代码

#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include "turtlesim/Pose.h"

using namespace ros;

std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    static tf2_ros::TransformBroadcaster T1;//这里切记静态！！！！！
    geometry_msgs::TransformStamped tf;
    tf.header.frame_id = "world";
    tf.header.stamp = Time::now();
    tf.child_frame_id = turtle_name;
    tf.transform.translation.x = pose->x;
    tf.transform.translation.y = pose->y;
    tf.transform.translation.z = 0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tf.transform.rotation.x = qtn.getX();
    tf.transform.rotation.y = qtn.getY();
    tf.transform.rotation.z = qtn.getZ();
    tf.transform.rotation.w = qtn.getW();
    T1.sendTransform(tf);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"dynamic");
    NodeHandle nh;
    if(argc!=2)
    {
        ROS_ERROR("输入错误，请重新输入");
        return 0;
    }
    else
    {
        turtle_name = argv[1];
    }
    Duration(0.4).sleep();
    Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);
    spin();
    return 0;
}

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="tf_test" type="new_turtlesim" name="turtle2" output="screen" />

    <node pkg="tf_test" type="turtlesim_pub" name="pub1" args="turtle1" output="screen" />
    <node pkg="tf_test" type="turtlesim_pub" name="pub2" args="turtle2" output="screen" />

</launch>

5.5.3 两只乌龟的坐标变换

可以复用5.3.1的编码

编码如下

#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

using namespace ros;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"control");
    NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    Rate rate(1);

    while(ok())
    {
        try
        {
            geometry_msgs::TransformStamped Son1toSon2 = buffer.lookupTransform("turtle2","turtle1",Time(0.0));
            ROS_INFO("%s相对于%s的偏移量:(%.2f,%.2f,%.2f)",
                    Son1toSon2.child_frame_id.c_str(),
                    Son1toSon2.header.frame_id.c_str(),
                    Son1toSon2.transform.translation.x,
                    Son1toSon2.transform.translation.y,
                    Son1toSon2.transform.translation.z
                    );
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        rate.sleep();
        spinOnce();
        
    }
    return 0;
}

这里只需要坐标变换，所以把坐标点部分删除

5.5.4 速度发布

在上一节的代码基础上进行修改

#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/Twist.h"

using namespace ros;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"control");
    NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    Rate rate(10);

    while(ok())
    {
        try
        {
            geometry_msgs::TransformStamped Son1toSon2 = buffer.lookupTransform("turtle2","turtle1",Time(0.0));
            // ROS_INFO("%s相对于%s的偏移量:(%.2f,%.2f,%.2f)",
            //         Son1toSon2.child_frame_id.c_str(),
            //         Son1toSon2.header.frame_id.c_str(),
            //         Son1toSon2.transform.translation.x,
            //         Son1toSon2.transform.translation.y,
            //         Son1toSon2.transform.translation.z
            //         );
            geometry_msgs::Twist twist;
            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);
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        rate.sleep();
        spinOnce();
        
    }
    return 0;
}

 其实就是不停根据位置发布新的速度信息（前面的系数决定了快慢）

代码详解：

        不需要切向速度和垂直速度，所以线速度只设置x。

        没有翻滚俯仰，所以角速度

5.6 rosbag

机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式，方式1：可以控制机器人运动，将机器人传感器感知到的数据时时处理，生成地图信息。方式2：同样是控制机器人运动，将机器人传感器感知到的数据留存，事后，再重新读取数据，生成地图信息。两种方式比较，显然方式2使用上更为灵活方便。

rosbag的本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。

5.6.1 rosbag的命令行使用

先启动小乌龟和控制节点

用命令启动rosbag的录制

cord：录制

-a：all，所有话题的消息

-o：output，输出

存放在bags下新建的hello.bag

结束直接Ctrl+c

info查看信息

path:        bags/hello_2024-12-30-14-09-35.bag      //存放路径
version:     2.0                                     //bag版本
duration:    40.1s                                   //录制持续时间
start:       Dec 30 2024 14:09:35.64 (1735538975.64) //开始时间
end:         Dec 30 2024 14:10:15.75 (1735539015.75) //结束时间
size:        365.0 KB                                //大小
messages:    5081                                    //消息量
compression: none [1/1 chunks]                       //是否压缩
types:       geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
             rosgraph_msgs/Log   [acffd30cd6b6de30f120938c17c593fb]
             turtlesim/Color     [353891e354491c51aabe32df673fb446]
             turtlesim/Pose      [863b248d5016ca62ea2e895ae5265cf9]
topics:      /rosout                    7 msgs    : rosgraph_msgs/Log   (2 connections)
             /rosout_agg                4 msgs    : rosgraph_msgs/Log  
             /turtle1/cmd_vel          81 msgs    : geometry_msgs/Twist
             /turtle1/color_sensor   2494 msgs    : turtlesim/Color    
             /turtle1/pose           2495 msgs    : turtlesim/Pose

rosbag play运行bag文件

其实就是把录制期间的操作复现一遍

关于rosbag的功能可以通过

rosbag record --help

查看

5.6.2 编码实现

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"

using namespace ros;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"bag1");
    NodeHandle nh;
    rosbag::Bag bag;
    //打开文件流
    bag.open("hello_bag",rosbag::BagMode::Write);
    std_msgs::String msg;
    msg.data = "hello xxxx";
    /*
        1.话题
        2.时间戳
        3.消息
    */
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.close();//关闭文件流
    return 0;
}

运行后发现目录下多了新文件

 5.6.3 读取编码实现

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"

using namespace ros;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    init(argc,argv,"read");
    NodeHandle nh;
    rosbag::Bag bag;
    //读的方式打开文件流
    bag.open("hello.bag",rosbag::BagMode::Read);
    // 5.读数据
    // 取出话题，时间戳和消息内容
    // 可以先获取消息的集合，再迭代取出消息的字段
    for(auto &&m : rosbag::View(bag))
    {
        std::string topic = m.getTopic();
        ros::Time time = m.getTime();
        std_msgs::StringPtr p = m.instantiate<std_msgs::String>();//返回值是指针类型
        ROS_INFO("解析的内容，话题:%s,时间戳:%.2f,消息值:%s",
                topic.c_str(),
                time.toSec(),
                p->data.c_str()
                );

    }
    bag.close();
    return 0;
}

5.7 rqt工具箱

5.7.1 基本使用

命令行输入rqt启用

rqt

启动乌龟节点

rosrun turtlesim turtlesim_node

 点击plugins选择所需的服务

5.7.2 rqt常用插件：rqt_graph

在rqt里打开node graph，查看节点关系

 5.7.3 rqt_console

先启动一个输出日志的节点

打开console

 可以根据需求进行过滤和筛选

5.7.4 rqt_plot

打开plot

添加自己要监视的数值

 5.7.5 rqt_bag

打开bag

选择要录制的话题

 

选择文件保存位置和文件名

蓝条是发布的时间

结束可以直接按×

第二个按键为打开文件

选择想要使用的文件

可以选择放映的开始和结束

右键发布才会表现