前言
此篇将会在之前已存在的几个坐标系（/world、/turtle1、/turtle2）的基础上再增加一个坐标系，相对来说，难度不大，主要是理解一些概念(脑子里面有3D场景的想象)，比如一个小车机器人处在世界坐标系（/world或/map）中，小车本身在基座底盘中心点形成一个坐标系（比如/base），小车身上前后左右又各有一个雷达，每个雷达又可以以各自中心点为原点形成各自的坐标系（/laser1、/laser2、/laser3、/laser4），在各自坐标系里方便处理一些测量数据，这样总共有1+1+4=6个坐标系了，而tf2系统则负责转换各个坐标系之间的关系。
在之前的章节里有提到过tf2树（tf2 tree）这个东西，它可以将机器人所在的大小环境的各坐标系之间的拓扑关系展示出来，如下图所示，但是tf2树有个规则，每个坐标系最多只允许有且只有一个父坐标系，其实也能好理解，比如雷达的坐标系只能是基于机器人底座的坐标系（或其他参考坐标系）才有意义，再比如一个小区所在的位置环境我们可以认为是一个世界坐标系，小区里面的每栋楼只能是在这个世界坐标系下才能有自己意义的定位（几号楼）坐标系，每栋楼里面的每室又是相对于该栋楼的定位（坐标系）才有意义。
动动手
写一个固定坐标系（帧）的广播
我们将在小海龟的例子里，为/turtle1新增一个子坐标系/carrot1，/carrot1将充当/turtle2的一个游动位置目标。
进入learning_tf2_cpp功能包的src路径下，执行如下命令下载fixed_frame_tf2_broadcaster.cpp:
$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp该源文件内容如下：
#include <chrono>
#include <functional>
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
using namespace std::chrono_literals;
class FixedFrameBroadcaster : public rclcpp::Node
{
public:
FixedFrameBroadcaster()
: Node("fixed_frame_tf2_broadcaster")
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
timer_ = this->create_wall_timer(
100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
}
private:
void broadcast_timer_callback()
{
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;
t.transform.rotation.x = 0.0;
t.transform.rotation.y = 0.0;
t.transform.rotation.z = 0.0;
t.transform.rotation.w = 1.0;
tf_broadcaster_->sendTransform(t);
}
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
rclcpp::shutdown();
return 0;
}代码的实现内容与之前的广播例子很相似，只有一个地方不一样，此处的转换数据是固定死的（所以叫fixed frame）。
代码分析
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;可以看到在这里，我们创建一个新的转换(从父级turtle1新的子级carrot1)。carrot1坐标系在turtle1坐标系的y轴上偏移了2米。
CMakeLists.txt
打开learning_tf2_cpp包下的CMakeLists.txt文件，将下面内容加进去，我们将fixed_frame_tf2_broadcaster.cpp编译完成后可执行文件的名字叫为fixed_frame_tf2_brodcaster:
add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
fixed_frame_tf2_broadcaster
geometry_msgs
rclcpp
tf2_ros
)
install(TARGETS
fixed_frame_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})重新写个launch文件
在launch文件夹下，创建一个新的启动文件turtle_tf2_fixed_frame_demo_launch.py:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('learning_tf2_cpp'), 'launch'),
'/turtle_tf2_demo_launch.py']),
)
return LaunchDescription([
demo_nodes,
Node(
package='learning_tf2_cpp',
executable='fixed_frame_tf2_broadcaster',
name='fixed_broadcaster',
),
])该启动文件引入了另外一个启动文件turtle_tf2_demo_launch.py，用来启动demo_nodes节点，文件的最后将会将carrot1坐标系（fixed_frame_tf2_broadcaster节点）添加到demo_nodes节点所在的例子中。
构建
执行如下依赖检查、构建步骤(回到工作空间根路径)。
$rosdep install -i --from-path src --rosdistro iron -y
$colcon build --packages-select learning_tf2_cpp运行
重开一个终端，进入工作空间根路径下先source下环境再运行启动文件。
$. install/setup.bash$ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py
我们执行如下命令查看下当前的tf2树，确认下/turtle1下面是不是多了一个/carrot1的子坐标系。
$ros2 run tf2_tools view_frames
也可以通过如下命令查看从/turtle1到/carrot1的转换数据。
$ros2 run tf2_ros tf2_echo turtle1 carrot1
如果我们通过turtle_teleop_key来控制turtle1，turtle2跟之前的例子一样也是跟随turtle1的轨迹游动，并没有像 我们计划的那样与turtle1在y轴上保持2米的距离，这是因为新增的carrot1坐标系不影响其他坐标系，监听者还是使用了之前定义的坐标系在游动（turtle2的target_frame还是turtle1而不是carrot1）。
我们有两种方法可以让turtle2朝着carrot1的目标值前进（而不是turtle1，虽然carrot1与turtle1也有着关系哈）。一种是直接在启动launch文件时直接指定target_frame参数值为carrot1，比如下面这样：
$ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py target_frame:=carrot1或者直接修改launch文件target_frame参数值为carrot1，两者效果是一样的。
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
...,
launch_arguments={'target_frame': 'carrot1'}.items(),
)我们再重新构建启动一下，对比一下前面的那张图，turtle2与turtle1不重合了，而是相差着点距离。
我们再通过turtle_teleop_key控制turtle1游动几下，看看turtle2的游动轨迹是怎样的。
写一个动态坐标系（帧）的广播
上面的例子将carrot1对于turtle1的y轴坐标值固定成了2米，下面的例子将turtle2的x和y轴的坐标值实现成随着turtle1的实际值而作一定规律的动态变化（当然也可以统一成turtle1一模一样的值，但是这个不是在之前就已经实现了吗，再这样就没什么新鲜感了）。
进入learning_tf2_cpp包的src路径下，执行如下命令下载源代码文件dynamic_frame_tf2_broadcaster.cpp:
$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp内容如下：
#include <chrono>
#include <functional>
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
using namespace std::chrono_literals;
const double PI = 3.141592653589793238463;
class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
DynamicFrameBroadcaster()
: Node("dynamic_frame_tf2_broadcaster")
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
timer_ = this->create_wall_timer(
100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
}
private:
void broadcast_timer_callback()
{
rclcpp::Time now = this->get_clock()->now();
double x = now.seconds() * PI;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = now;
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);
t.transform.translation.z = 0.0;
t.transform.rotation.x = 0.0;
t.transform.rotation.y = 0.0;
t.transform.rotation.z = 0.0;
t.transform.rotation.w = 1.0;
tf_broadcaster_->sendTransform(t);
}
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());
rclcpp::shutdown();
return 0;
}与fixed_frame_tf2_broadcaster.cpp的不同之处在于转换变量的x和y值的赋值不同了，此处是以x值(double x = now.seconds() * PI)为参数分别作个正弦和余弦变换。
CMakeLists.txt
修改CMakeLists.txt，添加dynamic_frame_tf2_broadcaster节点：
add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
dynamic_frame_tf2_broadcaster
geometry_msgs
rclcpp
tf2_ros
)
install(TARGETS
dynamic_frame_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})重新写个launch文件
在launch文件夹下新建turtle_tf2_dynamic_frame_demo_launch.py:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('learning_tf2_cpp'), 'launch'),
'/turtle_tf2_demo_launch.py']),
launch_arguments={'target_frame': 'carrot1'}.items(),
)
return LaunchDescription([
demo_nodes,
Node(
package='learning_tf2_cpp',
executable='dynamic_frame_tf2_broadcaster',
name='dynamic_broadcaster',
),
])构建
回到工作空间根路径下依次执行依赖检查、构建流程：
$rosdep install -i --from-path src --rosdistro iron -y$colcon build --packages-select learning_tf2_cpp运行
工作空间根路径下先source下环境再启动launch文件。
$. install/setup.bash$ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo_launch.py
turtle2(黄色) 小海龟在自顾自地按照命运脚本畅游了（也可以试试控制下turtle1的情况）。
本篇完。
853
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
