ros2笔记-5.3 C++中地图坐标系变换

本节继续跟小鱼老师学习5.3.需求背景：

地图坐标系为map，机器人坐标系为base link，目标点为target_point，已知map到base link之间的关系，map到target_point关系。要控制机器人到达目标点，就需要知道目标点 target_point和机器人base_link 之间的关系。

5.3.1 通过C++发布静态TF

目标点是固定的。采用静态TF发布map与target_point之间的关系。

在chapt/chapt5_ws/src下创建功能包

ros2 pkg create demo_cpp_tf --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs --license Apache-2.0

src/demo_cpp_tf/src 目录下新建文件：static_tf_broadcaster.cpp文件，代码如下：

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"  // 提供 tf2::Quaternion 类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2_ros/static_transform_broadcaster.h"  // 提供静态坐标广播器类

class staticTFBroadcaster : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
public:
    staticTFBroadcaster():Node("tf_broadcaster_node"){
        //创建静态广播发布器并进行发布
        broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        this->publish_tf();
    }

    void publish_tf(){
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->get_clock()->now();
        transform.header.frame_id ="map";
        transform.child_frame_id = "target_point";
        transform.transform.translation.x = 5.0;
        transform.transform.translation.y = 3.0;
        transform.transform.translation.z = 0.0;
        tf2::Quaternion quat;
        quat.setRPY(0,0,60*M_PI/180);//弧度制欧拉角转换四元数
        transform.transform.rotation = tf2::toMsg(quat);
        broadcaster_->sendTransform(transform);
    }
 
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<staticTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

主要逻辑就是定义类staticTFBroadcaster，声明了一个静态坐标广播的共享指针，在构造函数进行初始化，然后调用publish_tf进行静态TF发布，里面就是拼装参数，注意角度的转换，先转为弧度值，再把欧拉角转为四元数。

参照上一节课python版，静态发布只需要发布一次，ros2 为订阅者保留数据，新的订阅者可以直接获取保留的数据。

在CMakeLists.txt注册节点：

cmake_minimum_required(VERSION 3.8)
project(demo_cpp_tf)

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(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)
ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

install(TARGETS
  static_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME}
)


ament_package()

构建运行

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ colcon build
Starting >>> demo_cpp_tf
Starting >>> demo_python_tf
Finished <<< demo_python_tf [1.21s]                                                 
Finished <<< demo_cpp_tf [7.94s]                     

Summary: 2 packages finished [8.67s]
bohu@bohu-TM1701:~/chapt5/chapt5_ws$ source install/setup.bash 
bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run demo_cpp_tf static_tf_broadcaster 
^C[INFO

可以查看发布的话题

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 topic echo /tf_static
transforms:
- header:
    stamp:
      sec: 1736563886
      nanosec: 445839509
    frame_id: map
  child_frame_id: target_point
  transform:
    translation:
      x: 5.0
      y: 3.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.49999999999999994
      w: 0.8660254037844387
---

5.3.2 通过C++发布动态TF

动态TF对比静态就是需要有定时器不断发布。

src/demo_cpp_tf/src 目录下新建文件 dynamic_tf_broadcaster.cpp

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"  // 提供 tf2::Quaternion 类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2_ros/transform_broadcaster.h"  // 提供坐标广播器类
#include "chrono" //时间类头文件
using namespace std::chrono_literals;//时间单位 2s,5ms

class DynamicTFBroadcaster : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
public:
    DynamicTFBroadcaster():Node("dynamic_tf_broadcaster"){
        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        timer_ = create_wall_timer(10ms, std::bind(&DynamicTFBroadcaster::publish_tf,this));
    }

    void publish_tf(){
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->get_clock()->now();
        transform.header.frame_id ="map";
        transform.child_frame_id = "base_link";
        transform.transform.translation.x = 2.0;
        transform.transform.translation.y = 3.0;
        transform.transform.translation.z = 0.0;
        tf2::Quaternion quat;
        quat.setRPY(0,0,30*M_PI/180);//弧度制欧拉角转换四元数
        transform.transform.rotation = tf2::toMsg(quat);
        broadcaster_->sendTransform(transform);
    }
 
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<DynamicTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

小鱼老师讲动态的时候也讲了定时器的区别，吧静态的拿来改了下。基本上差不多。

在CMakeLists.txt注册static_tf_broadcaster节点，构建并运行

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ colcon build
Starting >>> demo_cpp_tf
Starting >>> demo_python_tf
Finished <<< demo_python_tf [1.18s]                                 
Finished <<< demo_cpp_tf [6.53s]                     

Summary: 2 packages finished [6.87s]
bohu@bohu-TM1701:~/chapt5/chapt5_ws$ source install/setup.bash 
bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run demo_cpp_tf dynamic_tf_broadcaster 

新开终端查看动态发布 ros2 topic ech /tf

transforms:
- header:
    stamp:
      sec: 1736565823
      nanosec: 655117538
    frame_id: map
  child_frame_id: base_link
  transform:
    translation:
      x: 2.0
      y: 3.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.25881904510252074
      w: 0.9659258262890683
---
transforms:
- header:
    stamp:
      sec: 1736565823
      nanosec: 665037284
    frame_id: map
  child_frame_id: base_link
  transform:
    translation:
      x: 2.0
      y: 3.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.25881904510252074
      w: 0.9659258262890683
---

如果此时，静态TF、动态TF都在运行，可以使用命令行查看base_link和target_poit之间的关系。

5.3.3 通过C++查询TF关系

src/demo_cpp_tf/src 目录下新建文件tf_listener.cpp,代码如下：

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"  // 提供 tf2::Quaternion 类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2/utils.h"  //提供tf2 :getEulerYPR 函数
#include "tf2_ros/buffer.h" //提供tf缓冲类buffer
#include "tf2_ros/transform_listener.h"  // 提供坐标广播器类
#include "chrono" //时间类头文件
using namespace std::chrono_literals;//时间单位 2s,5ms

class TFListener : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    rclcpp::TimerBase::SharedPtr timer_;
public:
    TFListener():Node("tf_listener"){
        buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
        timer_ = create_wall_timer(10ms, std::bind(&TFListener::getTransform,this));
    }

    void getTransform(){
        try{
            //
            const auto transform = buffer_->lookupTransform("base_link","target_point",
            this->get_clock()->now(),rclcpp::Duration::from_seconds(1.0f));
            const auto &translation = transform.transform.translation;
            const auto &roration = transform.transform.rotation;
            double y,p,r;
            tf2::getEulerYPR(roration,y,p,r);
            RCLCPP_INFO(get_logger(),"平移：（%f,%F,%f)",translation.x,translation.y,translation.z);
            RCLCPP_INFO(get_logger(),"旋转：（%f,%F,%f)",r,p,y);


        }catch(tf2::TransformException &ex){
            RCLCPP_WARN(get_logger(),"ex:%s",ex.what());
        }
    }
 
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<TFListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

基本结构跟动态的类似，小鱼老师视频也是吧动态拷过来改的。在构造函数对listener_初始化时候，第一个参数传的是Buffer对象的引用。随后创建定时器5秒调用一次。演示的 时候开始设置为1秒后面的查询函数getTransform就要1秒，这样会造成阻塞查不到数据.

其中获取结果 buffer_.lookup_transform  获取base_link到target_link的坐标变化,接口参数如下

  TF2_ROS_PUBLIC
  geometry_msgs::msg::TransformStamped
  lookupTransform(
    const std::string & target_frame, const std::string & source_frame,
    const rclcpp::Time & time,
    const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0)) const
  {
    return lookupTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout));
  }

还有一个点，        tf2::getEulerYPR(roration,y,p,r); 是 调用了utils函数来吧四元数转换为欧拉角。

所以roration是输入，y,pr 是输出。跟常见的入参出参格式不太一样，不太习惯。

最后，在CMakeLists.txt对tf_lisenter进行注册，重新构建后运行。

ros2 run demo_cpp_tf dynamic_tf_broadcaster

ros2 run demo_cpp_tf static_tf_broadcaster

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run demo_cpp_tf tf_listener 
[INFO] [1736573518.585501251] [tf_listener]: 平移：（2.598076,-1.500000,0.000000)
[INFO] [1736573518.585884521] [tf_listener]: 旋转：（0.000000,-0.000000,0.523599)
[INFO] [1736573518.596441302] [tf_listener]: 平移：（2.598076,-1.500000,0.000000)
[INFO] [1736573518.596568749] [tf_listener]: 旋转：（0.000000,-0.000000,0.523599)