添加一个坐标系（C++）
参考资料：https://docs.ros.org/en/iron/Tutorials/Intermediate/Tf2/Adding-A-Frame-Cpp.html。
目标
学习如何添加一个额外的坐标系到tf2中。
背景
本课程介绍如何添加额外的固定和动态的坐标系到转换树种。事实上，在tf2中添加坐标系类似于创建tf2广播器，但本示例将展示tf2的一些特点。
对于转换相关的许多任务来说，在本地坐标系内思考更容易。在激光扫描仪中心的一个坐标系中，最容易对激光扫描测量进行推理。tf2允许你为每一个传感器、连杆或关节定义本地坐标系。当从一个坐标系转换到另一个坐标系时，tf2将处理所有被引入的隐藏中间坐标系转换。
tf2树
tf2构建了坐标系的树状结构，不允许出现坐标系的闭环结构。这也意味着每个坐标系仅能有一个父坐标系，但可以有多个子坐标系。当前，我们的tf2树包含三个坐标系：world、turtle1和turtle2。两个turtle坐标系都是world坐标系的子坐标系。如果想添加新的坐标系到tf2，就需要从这三个坐标系选一个作为父坐标系。
任务
1、写固定坐标系广播器
添加新的坐标系carrot1，作为turtle1的子坐标系。该新的坐标系充当第二只海龟的位姿目标。
在learning_tf2_cpp功能包的src目录下创建源文件。直接下载源文件：
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp
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;
}
这份代码类似于tf2广播器课程示例，唯一的不同点是这里的转换不随时间改变。
1.1、代码解析
关键代码如下，其创建了从父坐标系turtle1到子坐标系carrot1的转换。carrot1坐标系相对于turtle1坐标系在y轴偏移了2米：
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;
1.2、CMakeLists.txt
修改CMakeLists.txt文件，添加内容：
add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
fixed_frame_tf2_broadcaster
geometry_msgs
rclcpp
tf2_ros
)
添加以下内容，便于ros2 run找到可执行程序：
install(TARGETS
fixed_frame_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})
1.3、写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',
),
])
1.4、编译
检查缺少的依赖：
rosdep install -i --from-path src --rosdistro iron -y
在工作空间根目录下编译功能包：
colcon build --packages-select learning_tf2_cpp
source设置文件：
. install/setup.bash
1.5、运行
启动示例：
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py
然后就可以看到新的carrot1坐标系出现在转换树中：
如果驱动第一只海龟运行，就会发现本示例跟前面课程中海龟行为是一样的，这是因为添加的新的坐标系不会影响其他的坐标系，tf2监听器仍使用之前定义的坐标系。
如果想让第二只海龟跟随胡萝卜，需要改变target_frame的值。有两种方式可以做到这一点。
第一种方式：启动时传递target_frame参数到launch文件
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py target_frame:=carrot1
第二种方式：修改launch文件。添加launch_arguments参数，值设为'target_frame': 'carrot1'
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
...,
launch_arguments={'target_frame': 'carrot1'}.items(),
)
然后，重新编译功能包，启动节点，就能看到第二只海龟跟随胡萝卜而不是第一只海龟了。
2、写动作坐标系广播器
上一节的额外坐标系是固定坐标系，不会随时间发生变化。但是，如果想发布一个运动的坐标系，可以写一个跟随时间变化的广播器。让我们修改carrot1坐标系使其随着时间改变与turtle1坐标系的关系。
可以下载动态坐标系广播器的代码：
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp
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;
}
2.1、代码解析
使用sin()和cos()函数来改变carrot1的偏移：
double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);
2.2、CMakeLists.txt
在CMakeLists.txt中添加内容：
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})
2.3、写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',
),
])
2.4、编译
首先，检查依赖：
rosdep install -i --from-path src --rosdistro iron -y
然后，编译：
colcon build --packages-select learning_tf2_cpp
source设置文件：
. install/setup.bash
2.5、运行
启动动态坐标系示例：
ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo_launch.py
将看到第二只海龟会跟随胡萝卜的位置。
总结
本课程中，学习了tf2转换树及其结构和特点。还学习了在本地坐标系下考虑问题更简单，学些了添加额外的固定和动态坐标系。
200
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
