ros2 高级组件其三之 TF2
1 前言与资料
本文我们介绍 ros2 高级组件之 TF2,TF 即 transform,坐标变换。早在 ros1 系列课程中,我们用了三篇博客,系统介绍了坐标变换的背景知识以及 API 编程。因此本文不再详述坐标变换的背景知识,读者可以自行阅读:ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一 进行补充。
本文将专注于 ros2 的 TF2 库的 API 编程,同样实现小乌龟2跟随小乌龟1样例,这个样例在:ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二中已经有实现。借此机会,我们可以比较两个 ros 版本的 TF 库 API 接口的不同之处。
除此之外,为了增加本文样例内容的丰富度,我们将采用之前学习的 ros2 component 机制:ROS2高效学习第十章 – ros2 高级组件其一之 component 合并进程启动,巩固已有的学习成果。
本文参考资料如下:
(1)ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一
(2)ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二
(3)ROS2高效学习第十章 – ros2 高级组件其一之 component 合并进程启动
(4)Introduction-To-Tf2
(5)Writing-A-Tf2-Broadcaster-Cpp
(6)Writing-A-Tf2-Listener-Cpp
(7)Writing-A-Tf2-Broadcaster-Py
(8)Writing-A-Tf2-Listener-Py
本系列博客汇总:ROS2 高效学习系列。
2 正文
2.1 learning_tf2_cpp
(1)创建 learning_tf2_cpp 软件包和相关文件
cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 learning_tf2_cpp --dependencies rclcpp_components geometry_msgs rclcpp tf2 tf2_ros turtlesim
cd learning_tf2_cpp
mkdir launch
touch launch/turtle_tf2_demo_launch.py
touch include/learning_tf2_cpp/turtle_tf_broadcaster.hpp
touch include/learning_tf2_cpp/turtle_tf_listener.hpp
touch include/learning_tf2_cpp/visibility_control.h
touch src/turtle_tf_broadcaster.cpp src/turtle_tf_listener.cpp
(2)编写 tf 广播器头文件:turtle_tf_broadcaster.hpp
#ifndef LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_
#define LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_
#include <functional>
#include <memory>
#include <string>
#include "learning_tf2_cpp/visibility_control.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"
namespace learning_tf2_cpp
{
class TurtleTfBroadcaster : public rclcpp::Node
{
public:
LEARNING_TF2_CPP_PUBLIC
explicit TurtleTfBroadcaster(const rclcpp::NodeOptions & options);
private:
void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg);
private:
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle_pose_sub_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::string turtle_name_;
};
} // namespace learning_tf2_cpp
#endif // LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_
(3)编写 tf 广播器:turtle_tf_broadcaster.cpp
#include "learning_tf2_cpp/turtle_tf_broadcaster.hpp"
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(learning_tf2_cpp::TurtleTfBroadcaster)
namespace learning_tf2_cpp
{
TurtleTfBroadcaster::TurtleTfBroadcaster(const rclcpp::NodeOptions & options)
: Node("turtle_tf_broadcaster", options) {
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
param_desc.description = "This parameter is used to pass in the name of the turtle";
turtle_name_ = this->declare_parameter<std::string>("turtle_name", "turtle1", param_desc);
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
std::string topic_name = "/" + turtle_name_ + "/pose";
turtle_pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10, std::bind(&TurtleTfBroadcaster::handle_turtle_pose, this, std::placeholders::_1));
}
// 小乌龟 TF 广播器的核心是通过订阅小乌龟的 Pose 信息,将 Pose 信息经过加工得到小乌龟相对 world 坐标系的 TF 坐标变换,
// 坐标变换主要分为两部分,一是平移变换,二是旋转变换,然后通过 TF 广播器将 TF 信息广播出去。
// 关于 Pose 信息,欧拉角以及四元数的理解,请参考:https://blog.csdn.net/cy1641395022/article/details/131236155
// 关于计算过程的理解,请参考:https://blog.csdn.net/cy1641395022/article/details/131236155
void TurtleTfBroadcaster::handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg) {
geometry_msgs::msg::TransformStamped tf_stamped;
tf_stamped.header.stamp = this->now();
tf_stamped.header.frame_id = "world";
tf_stamped.child_frame_id = this->turtle_name_;
tf_stamped.transform.translation.x = msg->x;
tf_stamped.transform.translation.y = msg->y;
tf_stamped.transform.translation.z = 0.0;
tf2::Quaternion tf_q;
tf_q.setRPY(0, 0, msg->theta);
tf_stamped.transform.rotation.x = tf_q.x();
tf_stamped.transform.rotation.y = tf_q.y();
tf_stamped.transform.rotation.z = tf_q.z();
tf_stamped.transform.rotation.w = tf_q.w();
this->tf_broadcaster_->sendTransform(tf_stamped);
}
} // namespace learning_tf2_cpp
(4)编写 tf 接收器头文件:turtle_tf_listener.hpp
#ifndef LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_
#define LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_
#include <functional>
#include <memory>
#include <string>
#include "learning_tf2_cpp/visibility_control.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/srv/spawn.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/exceptions.h"
namespace learning_tf2_cpp
{
class TurtleTfListener : public rclcpp::Node
{
public:
LEARNING_TF2_CPP_PUBLIC
explicit TurtleTfListener(const rclcpp::NodeOptions & options);
private:
void on_timer();
private:
bool turtle_spawning_service_ready_ = false;
bool turtle_spawned_ = false;
std::string target_frame_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawn_turtle_client_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr turtle_cmd_pub_;
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace learning_tf2_cpp
#endif // LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_
(5)编写 tf 接收器:turtle_tf_listener.cpp
#include "learning_tf2_cpp/turtle_tf_listener.hpp"
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(learning_tf2_cpp::TurtleTfListener)
namespace learning_tf2_cpp
{
TurtleTfListener::TurtleTfListener(const rclcpp::NodeOptions & options)
: Node("turtle_tf_listener", options) {
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
param_desc.description = "This parameter is used to pass in the name of the target turtle";
target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1", param_desc);
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
spawn_turtle_client_ = this->create_client<turtlesim::srv::Spawn>("spawn");
turtle_cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 10);
timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&TurtleTfListener::on_timer, this));
RCLCPP_INFO(this->get_logger(), "TurtleTfListener has been started");
}
// TF 监听器的逻辑是先生成一个小乌龟2,然后定时订阅在小乌龟2坐标系下,目标小乌龟的 TF 信息,
// 拿到 TF 信息后,计算得到小乌龟2的 Twist 的控制命令,然后以 1Hz 的频率发布到小乌龟2的 cmd_vel 话题上。
// Twist 的理解,参考:https://blog.csdn.net/cy1641395022/article/details/131236155
// 计算过程的理解,参考:https://blog.csdn.net/cy1641395022/article/details/131236155
void TurtleTfListener::on_timer() {
if (false == turtle_spawning_service_ready_) {
if (false == spawn_turtle_client_->service_is_ready()) {
RCLCPP_INFO(this->get_logger(), "Waiting for the spawn service to be ready");
return;
}
turtle_spawning_service_ready_ = true;
}
if (false == turtle_spawned_) {
auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
request->x = 2.0;
request->y = 2.0;
request->theta = 0.0;
request->name = "turtle2";
using ServiceResponseFuture = rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
auto response_received_callback = [this](ServiceResponseFuture future) {
auto result = future.get();
if (strcmp(result->name.c_str(), "turtle2") == 0) {
turtle_spawned_ = true;
RCLCPP_INFO(this->get_logger(), "Spawn turtle2 successfully");
} else {
RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
}
};
auto result = spawn_turtle_client_->async_send_request(request, response_received_callback);
return;
}
geometry_msgs::msg::TransformStamped tf_stamped;
try {
tf_stamped = tf_buffer_->lookupTransform("turtle2", target_frame_, tf2::TimePointZero);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what());
return;
}
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = 0.5 * std::sqrt(pow(tf_stamped.transform.translation.x, 2) + pow(tf_stamped.transform.translation.y, 2));
cmd_vel.angular.z = 1.0 * std::atan2(tf_stamped.transform.translation.y, tf_stamped.transform.translation.x);
RCLCPP_INFO(this->get_logger(), "send cmd_vel to turtle2: linear.x: %f, angular.z: %f", cmd_vel.linear.x, cmd_vel.angular.z);
turtle_cmd_pub_->publish(cmd_vel);
}
}
(6)编写 visibility_control.h:可见性头文件是为了让程序兼容 windows 和 linux,直接套模板改的,这里不再列出,参考:visibility_control
(7)编写 turtle_tf2_demo_launch.py
import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import Node
def generate_launch_description():
# 启动turtlesim节点,这个是基础环境
turtlesim_node = Node(
package='turtlesim',
executable='turtlesim_node',
name='turtlesim_node'
)
# 启动turtle1的tf广播节点,广播turtle1的tf
turtle1_tf_broadcaster = ComposableNodeContainer(
name='turtle1_tf_broadcaster',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='learning_tf2_cpp',
plugin='learning_tf2_cpp::TurtleTfBroadcaster',
name='turtle1_tf_bc',
parameters=[{'turtle_name': 'turtle1'}])
],
output='screen',
)
# 启动turtle2的tf广播节点,广播turtle2的tf
turtle2_tf_broadcaster = ComposableNodeContainer(
name='turtle2_tf_broadcaster',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='learning_tf2_cpp',
plugin='learning_tf2_cpp::TurtleTfBroadcaster',
name='turtle2_tf_bc',
parameters=[{'turtle_name': 'turtle2'}])
],
output='screen',
)
# 启动rviz2,加载turtlesim的 world 和两个小乌龟坐标系的可视化配置
turtlesim_world_rviz_config = os.path.join(
get_package_share_directory('learning_tf2_cpp'),
'config',
'turtle_tf.rviz'
)
turtlesim_world_rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', turtlesim_world_rviz_config]
)
# 启动tf监听节点,监听在 turtle2 坐标系下的 turtle1 的 tf,并转换为 turtle2 的控制指令
turtle2_listen_turtle1 = ComposableNodeContainer(
name='turtle2_listen_turtle1',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='learning_tf2_cpp',
plugin='learning_tf2_cpp::TurtleTfListener',
name='turtle_tf_ls',
parameters=[{'target_frame': 'turtle1'}])
],
output='screen',
)
return launch.LaunchDescription([turtlesim_node, turtle1_tf_broadcaster, turtle2_tf_broadcaster, turtlesim_world_rviz_node, turtle2_listen_turtle1])
(8)编写 CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(learning_tf2_cpp)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)
include_directories(include)
add_library(tf_broadcaster SHARED src/turtle_tf_broadcaster.cpp)
ament_target_dependencies(tf_broadcaster rclcpp rclcpp_components tf2 tf2_ros turtlesim)
rclcpp_components_register_nodes(tf_broadcaster "learning_tf2_cpp::TurtleTfBroadcaster")
add_library(tf_listener SHARED src/turtle_tf_listener.cpp)
ament_target_dependencies(tf_listener rclcpp rclcpp_components tf2 tf2_ros turtlesim)
rclcpp_components_register_nodes(tf_listener "learning_tf2_cpp::TurtleTfListener")
install(TARGETS
tf_broadcaster
tf_listener
LIBRARY DESTINATION lib
)
install(DIRECTORY
launch
config
DESTINATION share/${PROJECT_NAME}
)
ament_package()
(9)编译并运行
cd ~/colcon_ws/src
colcon build --packages-select learning_tf2_cpp
source install/local_setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py
# 再开一个窗口,光标留在这个窗口,使用方向键控制 turtle1 主动运动
ros2 run turtlesim turtle_teleop_key
2.2 learning_tf2_py
(1)learning_tf2_py 是 python 版本的 learning_tf2_cpp,这里不再详细讲述了,源码在:learning_tf2_py
2.3 三个 TF 的调试命令
# 参考 2.1 节,启动 learning_tf2_cpp 样例
# 可以得到当前树状坐标系的拓朴图
ros2 run tf2_tools view_frames
# 打印在 turtle2 坐标系下,turtle1 的 TF 值
ros2 run tf2_ros tf2_echo turtle2 turtle1
# 监控并统计 TF 信息收发的情况
ros2 run tf2_ros tf2_monitor turtle2 turtle1
3 总结
本文样例托管在本人的github上:learning_tf2_cpp,learning_tf2_py,欢迎复现提问。