ROS2 TF2坐标变换实操

最新推荐文章于 2025-05-27 17:43:00 发布

原创最新推荐文章于 2025-05-27 17:43:00 发布 · 764 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #机器人 #python #xml

ros2学习日志专栏收录该内容

18 篇文章

订阅专栏

生成乌龟（Exer01_spawn.cpp）：

坐标变换广播（Exer02_broadcaster.cpp）：

坐标获取以及速度消息发送（Exer03_tf_listener.cpp）：

需求：

编写程序实现，程序运行后会启动 turtlesim_node 节点，该节点会生成一个窗口，窗口中有一只原生乌龟（turtle1），紧接着再生成一只新的乌龟（turtle2），无论是 turtle1 静止或是被键盘控制运动时，turtle2 都会以 turtle1 为目标并向其运动。

思路：

按照一般的思路来说，当我们运行turtlesim_node 节点时，只会生成中央的一只乌龟，我们需要额外生成第二只乌龟，同时获取到第二只乌龟相对于第一只乌龟的坐标，这里就使用到了之前讲的广播broadcast，然后通过坐标给第二只乌龟赋一定的x方向和y方向的速度值大小。

生成乌龟需要用到服务端的spawn，需要的消息类型为turtlesim/srv/Spawn

为了节点能够同时启动，还要将他们写入launch文件中

准备工作：

首先创建C++和python的功能包

ros2 pkg create cpp09_exercise_review --build-type ament_cmake --dependencies rc
lcpp tf2 tf2_ros geometry_msgs turtlesim --node-name demo01_aaa

ros2 pkg create py09_exercise_review --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim --node-name demo01_aaa

然后再功能包目录下创建launch文件夹，再分别配置CMakeLists和setup.py

CMakeLists中加入

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

setup.py中的data_files修改为

    data_files=[
    ('share/ament_index/resource_index/packages',
        ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml'],),
        ('share/' + package_name, glob("launch/*.launch.xml")),
        ('share/' + package_name, glob("launch/*.launch.py")),
        ('share/' + package_name, glob("launch/*.launch.yaml")),
    ],

为了保证launch文件能够正常运行，在两个功能包的package.xml中加入

<exec_depend>ros2_launch</exec_depend>

为了确保正确，可以编译运行一下创建的节点，可以修改节点文件aaa，这里为了直接让系统直接配置一些文件创建的

C++实现

生成乌龟（Exer01_spawn.cpp）：

//包含头文件
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/srv/spawn.hpp"


using namespace std::chrono_literals;

// 自定义节点类
class Exer01_Spawn :public rclcpp::Node
{
    public:
    Exer01_Spawn():Node("Exer01_Spawn_node")
    {
        RCLCPP_INFO(this->get_logger(),"启动成功");
        // 创建可修改参数及获取
        this->declare_parameter("x",3.0);
        this->declare_parameter("y",3.0);
        this->declare_parameter("theta",0.0);
        this->declare_parameter("turtle_name","turtle2");

        x = this->get_parameter("x").as_double();
        y = this->get_parameter("y").as_double();
        theta = this->get_parameter("theta").as_double();
        turtle_name = this->get_parameter("turtle_name").as_string();
        
        // 创建服务点
        spawn_client_ = this->create_client<turtlesim::srv::Spawn>("/spawn");

    }
    bool connect_server(void)
    {
      while(!spawn_client_->wait_for_service(1s))
      {
        RCLCPP_INFO(this->get_logger(),"等待连接中...");
        if(time_out++ > 5 || !rclcpp::ok())
        {
          return false;
        }
      }
      return true;
    }
    rclcpp::Client<turtlesim::srv::Spawn>::FutureAndRequestId send_request(void)
    {
      auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
      request->x = x;
      request->y = y;
      request->theta = theta;
      request->name = turtle_name;

      // rclcpp::Client<turtlesim::srv::Spawn>::FutureAndRequestId 
      // async_send_request(std::shared_ptr<turtlesim::srv::Spawn_Request> request)
      return spawn_client_->async_send_request(request);
    }

    private:
      int8_t time_out = 0;
      double x,y,theta;
      std::string turtle_name;
      rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawn_client_;
};

int main(int argc,char **argv)
{
  // 初始化ROS2客户端
  rclcpp::init(argc,argv);
  // 等待连接
  auto client_ = std::make_shared<Exer01_Spawn>();
  bool flag = client_->connect_server();
  if(!flag){
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"连接失败");
    return 0;
  }
  // 发送请求
  auto response = client_->send_request();
  if(rclcpp::spin_until_future_complete(client_,response) == rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client_->get_logger(),"响应成功");
    std::string name = response.get()->name;
    if(name.empty())
    {
      RCLCPP_INFO(client_->get_logger(),"乌龟重名，生成失败");
    }
    else RCLCPP_INFO(client_->get_logger(),"乌龟重名，生成成功");
  }
  else
  {
    RCLCPP_ERROR(client_->get_logger(),"响应失败");
  }


  // rclcpp::spin(client_);
  //释放资源
  rclcpp::shutdown();
  return 0;
}

将生成乌龟加入launch文件中

from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类--------------
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取-----------------
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关-------------------
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关----------------------
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关----------------------
# from launch.event_handlers import OnProcessStart, OnProcessExit
# from launch.actions import ExecuteProcess, RegisterEventHandler,LogInfo
# 获取功能包下share目录路径-------
# from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    turtlesim = Node(package="turtlesim",executable="turtlesim_node")
    spawn = Node(package="cpp09_exercise_review",executable="exer01_spawn",
                 parameters=[{"turtle_name":"t2"}])
    return LaunchDescription([turtlesim,spawn])

坐标变换广播（Exer02_broadcaster.cpp）：

在这里需要分别将两个乌龟turtle和父坐标系world的相对坐标广播出去，为之后两个乌龟的相对坐标做准备；以便于之后的速度和角度计算

这里的坐标位置话题通过参数的形式进行获取，这样便于launch文件进行修改参数快速启动。

//包含头文件
#include "rclcpp/rclcpp.hpp"

#include "turtlesim/msg/pose.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"

// 自定义节点类
class TF_Dynamic_Broadcaster :public rclcpp::Node
{
    public:
    TF_Dynamic_Broadcaster():Node("TF_Dynamic_Broadcaster_node")
    {
        RCLCPP_INFO(this->get_logger(),"启动成功");
        this->declare_parameter("turtle","turtle1");
        turtle = this->get_parameter("turtle").as_string();

        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        
        pose_pub_ = this->create_subscription<turtlesim::msg::Pose>("/" + turtle + "/pose",10,                                                      
                                std::bind(&TF_Dynamic_Broadcaster::do_pose,this,std::placeholders::_1));

    }
    private:
    std::string turtle;
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_pub_;
    void do_pose(const turtlesim::msg::Pose &pose)
    {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->now();
        transform.child_frame_id = turtle;
        transform.header.frame_id = "world";
        
        transform.transform.translation.x = pose.x;
        transform.transform.translation.y = pose.y;
        transform.transform.translation.z = 0.0;

        tf2::Quaternion qtn;
        qtn.setRPY(0,0,pose.theta);
        transform.transform.rotation.w = qtn.w();
        transform.transform.rotation.x = qtn.x();
        transform.transform.rotation.y = qtn.y();
        transform.transform.rotation.z = qtn.z();

        broadcaster_->sendTransform(transform);
    }

};

int main(int argc,char **argv)
{
  // 初始化ROS2客户端
  rclcpp::init(argc,argv);
  //调用spin函数，传入指针
  rclcpp::spin(std::make_shared<TF_Dynamic_Broadcaster>());
  //释放资源
  rclcpp::shutdown();
  return 0;
}

坐标获取以及速度消息发送（Exer03_tf_listener.cpp）：

在这里创建广播接收必须的缓存池、监听器、以及定时发布消息所用到的发布者和定时器，由于没有使用到点坐标变换，所以没有涉及到之前学过的过滤器filter

//包含头文件
#include "rclcpp/rclcpp.hpp"

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/msg/twist.hpp"

using namespace std::chrono_literals;

// 自定义节点类
class Exer03_TFListener :public rclcpp::Node
{
    public:
    Exer03_TFListener():Node("Exer03_TFListener_node")
    {
        RCLCPP_INFO(this->get_logger(),"启动成功");
        this->declare_parameter("father_frame","turtle2");
        this->declare_parameter("child_frame","turtle1");
        father_frame = this->get_parameter("father_frame").as_string();
        child_frame = this->get_parameter("child_frame").as_string();

        // 创建缓存池和监听器
        buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
        pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/"+father_frame + "/cmd_vel",10);
        timer_ = this->create_wall_timer(1s,std::bind(&Exer03_TFListener::time_callback,this));
    }
    private:
    std::string father_frame;
    std::string child_frame;
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    void time_callback(){
      // 实现坐标变换
      try
      {
        // 发布速度指令       
        auto ts = buffer_->lookupTransform(father_frame,child_frame,tf2::TimePointZero);
        geometry_msgs::msg::Twist twist;
        twist.linear.x = 0.5 * sqrt(pow(ts.transform.translation.x,2) + pow(ts.transform.translation.y,2));
        twist.angular.z = 1.0 * atan2(ts.transform.translation.y,ts.transform.translation.x);

        pub_->publish(twist);
      }
      catch(const tf2::LookupException& e)
      {
        RCLCPP_INFO(this->get_logger(),"异常提示:%s",e.what());
      }
      
      
      
    }
};
int main(int argc,char **argv)
{
  // 初始化ROS2客户端
  rclcpp::init(argc,argv);
  //调用spin函数，传入指针
  rclcpp::spin(std::make_shared<Exer03_TFListener>());
  //释放资源
  rclcpp::shutdown();
  return 0;
}

launch文件：

from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类--------------
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取-----------------
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关-------------------
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关----------------------
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关----------------------
# from launch.event_handlers import OnProcessStart, OnProcessExit
# from launch.actions import ExecuteProcess, RegisterEventHandler,LogInfo
# 获取功能包下share目录路径-------
# from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 将t2修改为变量
    t2 = DeclareLaunchArgument(name="turtle2_name",default_value="t2")


    turtlesim = Node(package="turtlesim",executable="turtlesim_node")
    spawn = Node(package="cpp09_exercise_review",executable="exer01_spawn",
                 parameters=[{"turtle_name":LaunchConfiguration("turtle2_name")}])
    # 分别广播两只乌龟的坐标变换
    broadcaster1 = Node( package="cpp09_exercise_review",
                        executable="exer02_tf_broadcaster",
                        parameters=[{"turtle":"turtle1"}],
                        name="broadcaster1")
    broadcaster2 = Node( package="cpp09_exercise_review",
                        executable="exer02_tf_broadcaster",
                        parameters=[{"turtle":LaunchConfiguration("turtle2_name")}],
                        name="broadcaster2")
    # 创建监听节点并发送速度消息
    listener = Node(package="cpp09_exercise_review",
                    executable="exer03_tf_listener",
                    parameters=[{"father_frame":LaunchConfiguration("turtle2_name"),"child_frame":"turtle1"}])
    
    return LaunchDescription([t2,turtlesim,spawn,broadcaster1,broadcaster2,listener])

这里运行节点turtlesim以及自己创建的第二个乌龟节点，然后创建了两个广播，分别广播第一个乌龟和第二个乌龟的坐标变换，listener负责将两个乌龟的相对坐标获取到并给第二个乌龟赋予一定的线速度和角速度。

python实现

和c++类似

生成乌龟（）：

#包含头文件
import rclpy
from rclpy.node import Node

from turtlesim.srv import Spawn
# 自定义节点
class Exer01_Spawn(Node):
    def __init__(self):
        super().__init__("Exer01_Spawn_node")

        self.get_logger().info("启动成功")
        self.declare_parameter("x",3.5)
        self.declare_parameter("y",3.5)
        self.declare_parameter("theta",1.57)
        self.declare_parameter("turtle_name","t2")

        self.x = self.get_parameter("x").get_parameter_value().double_value
        self.y = self.get_parameter("y").get_parameter_value().double_value
        self.theta = self.get_parameter("theta").get_parameter_value().double_value
        self.turtle_name = self.get_parameter("turtle_name").get_parameter_value().string_value

        # 创建服务客户端
        self.client_ = self.create_client(Spawn,"/spawn")
        
        # 连接服务
        while not self.client_.wait_for_service(1.0):
            self.get_logger().info("等待连接中")

    def send_request(self):
        
        request = Spawn.Request()
        
        request.x  = self.x
        request.y = self.y
        request.theta = self.theta
        request.name = self.turtle_name

        self.future = self.client_.call_async(request)


def main():
    # 初始化客户端
    rclpy.init()

    spawn = Exer01_Spawn()
    spawn.send_request()
    rclpy.spin_until_future_complete(spawn,spawn.future)

    # 处理响应
    response = spawn.future.result()
    if len(response.name) == 0:
        spawn.get_logger().info("乌龟重名")
    else:
        spawn.get_logger().info("生成成功")

    # 释放资源
    rclpy.shutdown()

if __name__=='__main__':
   main()

坐标变换广播：

#包含头文件
import rclpy
from rclpy.node import Node

from rclpy.logging import get_logger
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations
from turtlesim.msg import Pose

# 自定义节点
class TF_Dynamic_Broadcaster(Node):
    def __init__(self):
        super().__init__("TF_Dynamic_Broadcaster_node")
        self.get_logger().info("启动成功")
        self.declare_parameter("turtle","turtle1")
        self.turtle = self.get_parameter("turtle").get_parameter_value().string_value
        self.broadcaster_ = StaticTransformBroadcaster(self)
        self.pose_pub_ = self.create_subscription(Pose,"/" + self.turtle +"/pose",self.do_pose,10)

    def do_pose(self,pose:Pose):
        transform = TransformStamped()
        
        transform.child_frame_id = self.turtle
        transform.header.frame_id ="world" 
        transform.header.stamp = self.get_clock().now().to_msg()
        
        transform.transform.translation.x = pose.x
        transform.transform.translation.y = pose.y
        transform.transform.translation.z = 0.0

        qtn = tf_transformations.quaternion_from_euler(0.0,0.0,pose.theta)
        transform.transform.rotation.x = qtn[0]
        transform.transform.rotation.y = qtn[1]
        transform.transform.rotation.z = qtn[2]
        transform.transform.rotation.w = qtn[3]

        self.broadcaster_.sendTransform(transform)


def main():
    # 初始化客户端
    rclpy.init()
    # 调用spin传入节点对象
    rclpy.spin(TF_Dynamic_Broadcaster())
    # 释放资源
    rclpy.shutdown()

if __name__=='__main__':
   main()

坐标获取以及速度消息发送：

#包含头文件
import rclpy
from rclpy.node import Node

from tf2_ros import Buffer
from tf2_ros import TransformListener

from geometry_msgs.msg import Twist
from rclpy.time import Time
from math import sqrt
from math import atan2

# 自定义节点
class My_node(Node):
    def __init__(self):
        super().__init__("My_node_node")
        self.get_logger().info("启动成功")
        self.declare_parameter("father_frame","turtle2")
        self.declare_parameter("child_frame","turtle1")
        self.father_frame = self.get_parameter("father_frame").get_parameter_value().string_value
        self.child_frame = self.get_parameter("child_frame").get_parameter_value().string_value

        self.buffer_ = Buffer()
        self.listener_ = TransformListener(self.buffer_,self)
        self.cmd_pub = self.create_publisher(Twist,self.father_frame + "/cmd_vel",10)
        self.timer_ = self.create_timer(1.0,self.time_callback)
    def time_callback(self):
        if  self.buffer_.can_transform(self.father_frame,self.child_frame,Time()):
            twist = Twist()
            ts = self.buffer_.lookup_transform(self.father_frame,self.child_frame,Time())
            twist.linear.x = 0.5 * sqrt(pow(ts.transform.translation.x,2) + pow(ts.transform.translation.y,2))  
            twist.angular.z = atan2(ts.transform.translation.y,ts.transform.translation.x)
            self.cmd_pub.publish(twist)
        else:
            self.get_logger().error("转换失败")
            return 

def main():
    # 初始化客户端
    rclpy.init()
    # 调用spin传入节点对象
    rclpy.spin(My_node())
    # 释放资源
    rclpy.shutdown()

if __name__=='__main__':
   main()

xml文件：

因为C++中已经写了launch的python形式，这里换成xml形式，顺便复习一下之前的内容

<launch>

    <arg name="turtle2_name" default="t2"/>
    
    <!-- turtlesim_node -->
    <node pkg="turtlesim" exec="turtlesim_node"/>

    <!-- 第二只乌龟节点 -->
    <node pkg="py09_exercise_review" exec="exer01_spawn" name="spawn1">
        <param name="x" value="1.0"/>
        <param name="y" value="3.0"/>
        <param name="turtle_name" value="$(var turtle2_name)"/>
    </node>

    <!-- 广播两只乌龟坐标变换 -->
    <node pkg="py09_exercise_review" exec="exer02_tf_broadcaster" name="listener1">
        <param name="turtle" value="turtle1"/>
    </node>

    <node pkg="py09_exercise_review" exec="exer02_tf_broadcaster" name="listener2">
        <param name="turtle" value="$(var turtle2_name)"/>
    </node>

    <node pkg="py09_exercise_review" exec="exer03_tf_listener">
        <param name="father_frame" value="$(var turtle2_name)"/>
        <param name="child_frame" value="turtle1"/>
    </node>
</launch>

现象和C++相同

参考：B站赵虚左老师的ROS2教程