《ROS2 机器人开发 从入门道实践》 鱼香ROS2——第5章内容

目录

第5章 ROS常用开发工具

5.1 坐标变换工具介绍

5.1.1 通过命令行使用TF

5.1.2 对TF原理的简单探究

5.2 Python中的手眼坐标变换

5.2.1 通过Python发布静态TF

5.2.2 通过Python发布动态TF

5.2.3 通过Python查询TF关系

5.3 C++中的地图坐标系变化

5.3.1 通过C++发布静态TF

5.3.2 通过C++发布动态TF

5.3.3 通过C++查询TF关系

5.4 常用可视化工具rqt与RViz

5.4.1 GUI框架rqt

5.4.2 数据可视化工具RViz

5.5 数据记录工具ros2 bag

5.6 ROS2基础之Git进阶

5.6.1 查看修改内容

5.6.2 学会撤销代码

5.6.3 进阶掌握Git分支

---

第5章 ROS常用开发工具

5.1 坐标变换工具介绍

5.1.1 通过命令行使用TF

1. 新打开一个终端，建立base_link到base_laser的坐标关系

ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0.0 --z 0.2 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id base_laser

2. 新打开一个终端，建立base_laser到wall_point的坐标关系

ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0.0 --z 0.2 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_laser --child-frame-id wall_point

3. 新打开一个终端，查询base_link到wall_point的坐标关系，会一直运行这个查询程序

ros2 run tf2_ros tf2_echo base_link wall_point

4. 安装3D旋转可视化

sudo apt install ros-humble-mrpt2 -y 无法定位软件包Unable to locate package ros-humble-mrpt2

使用 sudo apt install mrpt-apps 可以安装成功

5. 打开3d旋转可视化窗口

3d-rotation-converter

6. 查看TF树，导出至PDF文件中，需要同时运行1和2步的两个终端指令

ros2 run tf2_tools view_frames

5.1.2 对TF原理的简单探究

1. ros2 topic list查看话题

/parameter_events
/rosout
/tf_static

2. 查看话题tf_static，ros2 topic info /tf_static

Type: tf2_msgs/msg/TFMessage
Publisher count: 2
Subscription count: 0

3. 查看消息接口的定义，ros2 interface show tf2_msgs/msg/TFMessage

geometry_msgs/TransformStamped[] transforms
    #
    #
    std_msgs/Header header 成员变量
        builtin_interfaces/Time stamp 时间戳
            int32 sec
            uint32 nanosec
        string frame_id 父坐标系名字
    string child_frame_id 子坐标系名字
    Transform transform
        Vector3 translation
            float64 x
            float64 y
            float64 z
        Quaternion rotation
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1

4. 订阅话题，ros2 topic echo /tf_static，显示话题内容

tf_static是静态坐标发布，tf是动态坐标发布

5.2 Python中的手眼坐标变换

5.2.1 通过Python发布静态TF

1. 安装两个库

sudo apt install ros-$ROS_DISTRO-tf-transformations

2.创建新的文件夹chapt5/chapt5_ws/src，在src下创建功能包

ros2 pkg create --build-type ament_python --dependencies rclpy geometry_msgs tf_ros tf_transformations --license Apache-2.0 demo_python_tf

3. 新建文件chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf/static_tf_broadcast.py

import rclpy
from rclpy.node import Node
# 静态坐标发布器
from tf2_ros import StaticTransformBroadcaster
# 消息接口
from geometry_msgs.msg import TransformStamped
# 欧拉角转四元数
from tf_transformations import quaternion_from_euler
# 用于将角度转弧度
import math

class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        # 创建一个坐标发布器对象
        self.static_broadcaster_ = StaticTransformBroadcaster(self)
        self.publish_static_tf()

    # 发布静态TF 从base_link到camera_link的坐标关系
    def publish_static_tf(self):
        # 创建一个消息接口对象
        transform = TransformStamped()
        # 对消息接口transform进行成员赋值
        # 父坐标系名称
        transform.header.frame_id = 'base_link'
        # 子坐标系名称
        transform.child_frame_id = 'camera_link'
        # 时间
        transform.header.stamp = self.get_clock().now().to_msg()
        
        # 坐标关系
        transform.transform.translation.x = 0.5
        transform.transform.translation.y = 0.3
        transform.transform.translation.z = 0.6
        # 角度换算，欧拉角转四元数，返回一个元组q = x,y,z,w
        q = quaternion_from_euler(math.radians(180),0,0)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]

        # 发布静态坐标关系
        self.static_broadcaster_.sendTransform(transform)
        self.get_logger().info(f'发布静态坐标：{transform}')

def main():
    rclpy.init()
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

4. 修改setup.py

    entry_points={
        'console_scripts': [
            'static_tf_broadcast = demo_python_tf.static_tf_broadcast:main',
            ],
    },

5. 新打开终端，查看结果

ros2 topic list
ros2 topic echo /tf_static

5.2.2 通过Python发布动态TF

1. 新建文件chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf/dynamic_tf_broadcast.py

import rclpy
from rclpy.node import Node
# 动态坐标发布器
from tf2_ros import TransformBroadcaster
# 消息接口
from geometry_msgs.msg import TransformStamped
# 欧拉角转四元数
from tf_transformations import quaternion_from_euler
# 用于将角度转弧度
import math

class TFBroadcaster(Node):
    def __init__(self):
        super().__init__('tf_broadcaster')
        # 创建一个坐标发布器对象
        self.broadcaster_ = TransformBroadcaster(self)
        # 定时器，定时发布动态TF消息
        self.timer_ = self.create_timer(0.01, self.publish_tf)

    # 发布动态TF 从camera_link到bottle_link的坐标关系
    def publish_tf(self):
        # 创建一个消息接口对象
        transform = TransformStamped()
        # 对消息接口transform进行成员赋值
        # 父坐标系名称
        transform.header.frame_id = 'camera_link'
        # 子坐标系名称
        transform.child_frame_id = 'bottle_link'
        # 时间
        transform.header.stamp = self.get_clock().now().to_msg()
        
        # 坐标关系
        transform.transform.translation.x = 0.2
        transform.transform.translation.y = 0.3
        transform.transform.translation.z = 0.5
        # 角度换算，欧拉角转四元数，返回一个元组q = x,y,z,w
        q = quaternion_from_euler(0,0,0)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]

        # 发布静态坐标关系
        self.broadcaster_.sendTransform(transform)
        self.get_logger().info(f'发布动态坐标：{transform}')

def main():
    rclpy.init()
    node = TFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

2.修改setup.py

    entry_points={
        'console_scripts': [
            'static_tf_broadcast = demo_python_tf.static_tf_broadcast:main',
            'dynamic_tf_broadcast = demo_python_tf.dynamic_tf_broadcast:main'
        ],
    },

3. 查看动态TF发布频率ros2 topic hz /tf

5.2.3 通过Python查询TF关系

1. 新建文件chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf/tf_listener.py

import rclpy
from rclpy.node import Node
# 导入坐标变换监听器
from tf2_ros import TransformListener,Buffer
# 四元数转欧拉角
from tf_transformations import euler_from_quaternion

class TFBroadcaster(Node):
    def __init__(self):
        super().__init__('tf_broadcaster')
        self.buffer_ = Buffer()
        # 创建一个坐标发布器对象
        self.listener = TransformListener(self.buffer_, self)
        # 定时器，定时发布动态TF消息
        self.timer_ = self.create_timer(1.0, self.get_transform)

    # 定时获取坐标关系 从camera_link到bottle_link的坐标关系
    def get_transform(self):
        try:
            # 查询坐标关系，参数：父、子坐标名称，查询时间（最新时间），超时时间
            result = self.buffer_.lookup_transform('base_link','bottle_link',
                                                   rclpy.time.Time(seconds = 0.0),rclpy.time.Duration(seconds = 1.0))
            transform = result.transform
            self.get_logger().info(f'平移: {transform.translation}')
            self.get_logger().info(f'旋转: {transform.rotation}')
            rotation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w])
            self.get_logger().info(f'欧拉角RPY: {rotation_euler}')
            transform.translation
        except Exception as e:
            self.get_logger().info(f'异常原因: {str(e)}')
            pass

def main():
    rclpy.init()
    node = TFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

2. 同时运行5.2.1、5.2.2、5.2.3三个可执行文件

5.3 C++中的地图坐标系变化

5.3.1 通过C++发布静态TF

1. 在chapt5/chapt5_ws/src下创建功能包

ros2 pkg create demo_cpp_tf --build-type ament_cmake --dependencies rclcpp tf2_ros geometry_msgs tf2_geometry_msgs --license Apache-2.0

2. 进入功能包的src目录下，创建文件static_tf_broadcaster.cpp

// 引入geometry_msgs消息包中的TransformStamped消息类型的头文件，
// 用于构建包含坐标变换信息的消息，比如坐标平移、旋转等信息
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
// 四元数tf2：Quaternion类
#include "tf2/LinearMath/Quaternion.h"
// tf2库与geometry_msgs消息类型转换的函数，
// 例如将tf2的四元数类型转换为geometry_msgs中表示旋转的消息类型
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
// 静态坐标广播器类
#include "tf2_ros/static_transform_broadcaster.h"

// 定义一个名为StaticTFBroadcaster的类，它继承自rclcpp::Node类
class StaticTFBroadcaster : public rclcpp::Node {
   private:
    // 一个指向tf2_ros::StaticTransformBroadcaster类型的智能指针，用于发布静态坐标变换消息
    std::shared_ptr broadcaster_;

   public:
    // 构造函数首先调用父类（rclcpp::Node）的构造函数，传入节点名称初始化节点
    StaticTFBroadcaster() : Node("static_tf_broadcaster") {
        // 传入当前节点self作为参数，将当前节点指针（this）传递给它，
        // 以便广播器能与当前节点关联起来进行消息发布操作
        broadcaster_ =
            std::make_shared(this);
        // 静态信息发布，发布一次即可
        this->publish_tf();
    };
    void publish_tf() {
        // 创建一个TransformStamped类型的消息对象transform，用于填充坐标变换的详细信息
        geometry_msgs::msg::TransformStamped transform;
        // 设置消息头的时间戳为当前节点的时钟时间
        transform.header.stamp = this->get_clock()->now();
        // 设置坐标变换的父坐标系、子坐标系，表明从map到target_point坐标系的变换
        transform.header.frame_id = "map";
        transform.child_frame_id = "target_point";
        transform.transform.translation.x = 5.0;
        transform.transform.translation.y = 3.0;
        transform.transform.translation.z = 0.0;
        // 创建一个tf2的四元数对象q
        tf2::Quaternion q;
        q.setRPY(0.0, 0.0, 60 * M_PI / 180.0);
        // 通过tf2::toMsg(q)将tf2的四元数转换为geometry_msgs中对应的旋转消息类型
        transform.transform.rotation = tf2::toMsg(q);
        // 发布消息
        this->broadcaster_->sendTransform(transform);
    }
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    // 创建StaticTFBroadcaster类的智能指针node，实例化节点
    auto node = std::make_shared();
    rclcpp::spin(node);
    rclcpp::shutdown();
}

3. 修改CMakeLists.txt

add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)

ament_target_dependencies(static_tf_broadcaster rclcpp tf2_ros geometry_msgs tf2_geometry_msgs)

install(TARGETS static_tf_broadcaster
DESTINATION lib/${PROJECT_NAME})

4. 构建功能包、设置环境，运行节点

5. 新开终端ros2 topic list查询话题，ros2 topic echo /tf_static查看话题内容

5.3.2 通过C++发布动态TF

1. src目录下新建文件dynamic_tf_broadcaster.cpp

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
// 动态坐标广播器类
#include "chrono"
#include "tf2_ros/transform_broadcaster.h"
using namespace std::chrono_literals;  // 使用s，ms

class TFBroadcaster : public rclcpp::Node {
   private:
    std::shared_ptr broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;

   public:
    TFBroadcaster() : Node("tf_broadcaster") {
        broadcaster_ = std::make_shared(this);
        timer_ = this->create_wall_timer(
            100ms, std::bind(&TFBroadcaster::publish_tf, this));
    }
    void publish_tf() {
        // 创建一个TransformStamped类型的消息对象transform，用于填充坐标变换的详细信息
        geometry_msgs::msg::TransformStamped transform;
        // 设置消息头的时间戳为当前节点的时钟时间
        transform.header.stamp = this->get_clock()->now();
        // 设置坐标变换的父坐标系、子坐标系，表明从map到target_point坐标系的变换
        transform.header.frame_id = "map";
        transform.child_frame_id = "base_link";
        transform.transform.translation.x = 2.0;
        transform.transform.translation.y = 3.0;
        transform.transform.translation.z = 0.0;
        // 创建一个tf2的四元数对象q
        tf2::Quaternion q;
        q.setRPY(0.0, 0.0, 30 * M_PI / 180.0);
        // 通过tf2::toMsg(q)将tf2的四元数转换为geometry_msgs中对应的旋转消息类型
        transform.transform.rotation = tf2::toMsg(q);
        // 发布消息
        this->broadcaster_->sendTransform(transform);
    }
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    // 创建TFBroadcaster类的智能指针node，实例化节点
    auto node = std::make_shared();
    rclcpp::spin(node);
    rclcpp::shutdown();
}

 2. 修改CMakeLists.txt

add_executable(dynamic_tf_broadcaster src/dynamic_tf_broadcaster.cpp)

ament_target_dependencies(dynamic_tf_broadcaster rclcpp tf2_ros geometry_msgs tf2_geometry_msgs)

install(TARGETS static_tf_broadcaster dynamic_tf_broadcaster
DESTINATION lib/${PROJECT_NAME})

3. 运行ros2 run demo_cpp_tf dynamic_tf_broadcaster

4. 新建终端查看话题ros2 topic list，查看话题内容ros2 topic echo /tf

5.3.3 通过C++查询TF关系

1. 新建文件tf_listener.cpp

#include "chrono"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"  // 提供了四元数转欧拉角
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"              // 提供buffer_存储坐标信息
#include "tf2_ros/transform_listener.h"  // 坐标监听类
using namespace std::chrono_literals;    // 使用s，ms

class TFListener : public rclcpp::Node {
   private:
    std::shared_ptr listener_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr buffer_;

   public:
    TFListener() : Node("tf_listener") {
        buffer_ = std::make_shared(this->get_clock());
        // tf2_ros::TransformListener会自从订阅静态和动态话题，并保存到buffer_中
        this->listener_ =
            std::make_shared(*buffer_, this);
        timer_ = this->create_wall_timer(
            1s, std::bind(&TFListener::getTransform, this));
    }
    // 查询坐标信息
    void getTransform() {
        try {
            const auto transform = buffer_->lookupTransform(
                "base_link", "target_point", this->get_clock()->now(),
                rclcpp::Duration::from_seconds(1.0f));
            // 获取查询结果
            auto translation = transform.transform.translation;
            auto rotation = transform.transform.rotation;
            double y, p, r;
            // 接收rotation，自动转化坐标到y,p,r
            tf2::getEulerYPR(rotation, y, p, r);
            RCLCPP_INFO(get_logger(), "平移:(%f,%f,%f)", translation.x,
                        translation.y, translation.z);
            RCLCPP_INFO(get_logger(), "旋转:(%f,%f,%f)", y, p, r);
        } catch (const std::exception& e) {
            RCLCPP_WARN(get_logger(), "%s", e.what());
        }
    }
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    // 创建TFListener类的智能指针node，实例化节点
    auto node = std::make_shared();
    rclcpp::spin(node);
    rclcpp::shutdown();
}

rm -rf ~/.config/ros.org/rqt_gui.ini

2. 同时运行5.3.1和5.3.2和5.3.3三个可执行文件

5.4 常用可视化工具rqt与RViz

5.4.1 GUI框架rqt

1. 安装rqt-tf-tree插件

sudo apt install ros-humble-rqt-tf-tree -y

2. 更新配置文件

rm -rf ~/.config/ros.org/rqt_gui.ini

3. 打开rqt，Plugins、Visualization、TF Tree

4. 运行5.3.1可执行文件，刷新rqt

终端中在后台运行程序，在后面加个 & ，如ros2 run demo_cpp_tf dynamic_tf_broadcaster &

5.4.2 数据可视化工具RViz

1. 终端打开rviz，rviz2

2. 将rviz配置文件保存

3. 终端rviz2 ./rviz_tf.rviz

5.5 数据记录工具ros2 bag

记录小海龟记录，重新播放轨迹

1. 终端打开小海龟，ros2 run turtlesim turtlesim_node

2. 新开终端，打开小海龟的键盘控制节点 ros2 run turtlesim turtle_teleop_key

3. 新开终端，记录话题 ros2 bag record /turtle1/cmd_vel

4. 查看ros记录包，ls ros*

5. 打开ros记录包， cat rosbag2_2025_01_11-20_46_38/metadata.yaml

6. 播放记录，ros2 bag play rosbag2_2025_01_11-20_46_38/，(和5不一样),需要另一个终端打开小海龟

5.6 ROS2基础之Git进阶

5.6.1 查看修改内容

git diff

git diff 文件名

5.6.2 学会撤销代码

未git add时，git checkout 文件名，取消修改

git add时，git reset 文件名，将文件从暂存区删除

git commit时，git log查看版本，git reset 版本号，进行恢复

5.6.3 进阶掌握Git分支

git branch查看分支

创建新分支，git branch 新分支名

切换到新分支，git checkout 新分支名

合并分支的改变内容，在原分支的基础上，git merge 新分支名

删除分支 git branch -D 分支名