添加一个坐标系(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转换树及其结构和特点。还学习了在本地坐标系下考虑问题更简单,学些了添加额外的固定和动态坐标系。