赞
踩
本文我们介绍 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
(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
(1)learning_tf2_py 是 python 版本的 learning_tf2_cpp,这里不再详细讲述了,源码在:learning_tf2_py
# 参考 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
本文样例托管在本人的github上:learning_tf2_cpp,learning_tf2_py,欢迎复现提问。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。