ROS2 学习笔记5 坐标变换

于 2025-05-04 16:54:39 发布
阅读量185 收藏
点赞数 1
CC 4.0 BY-SA版权
文章标签： 学习 笔记
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/tech1001/article/details/147595436

1. 坐标变换广播

安装

sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations
sudo apt install python3-pip
pip3 install transforms3d

依赖
c++：rclcpp tf2 tf2_ros geometry_msgs turtlesim
python：rclpy tf_transformation tf2_ros geometry_msgs turtlesim

命令实现

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 1.0 --y 1.0 --z 1.0

静态广播

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
    
class TFStaticBroadCaster: public rclcpp::Node{
public:
  TFStaticBroadCaster(char * argv[]):Node("tf_static_broadcaster_node_cpp"){
    broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
    pub_static_tf(argv);
  }
private:
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
  void pub_static_tf(char * argv[]){
    geometry_msgs::msg::TransformStamped transfrom;
    transfrom.header.stamp = this->now();
    transfrom.header.frame_id = argv[7];
    transfrom.child_frame_id = argv[8];
    transfrom.transform.translation.x = atof(argv[1]);
    transfrom.transform.translation.y = atof(argv[2]);
    transfrom.transform.translation.z = atof(argv[3]);
    tf2::Quaternion qtn;
    qtn.setRPY(atof(argv[4]),atof(argv[5]),atof(argv[6]));
    transfrom.transform.rotation.x = qtn.x();
    transfrom.transform.rotation.y = qtn.y();
    transfrom.transform.rotation.z = qtn.z();
    transfrom.transform.rotation.w = qtn.w();
    broadcaster_->sendTransform(transfrom);
  }
};
    
int main(int argc, char * argv[])
{
  if (argc != 9)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"非法传参");
    return 1;
  }
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<TFStaticBroadCaster>(argv));
  rclcpp::shutdown();
  return 0;
}

import rclpy
from rclpy.node import Node
import sys
from rclpy.logging import get_logger
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class TFStaticBroadCasterPy(Node):
    def __init__(self,argv):
        super().__init__("tf_static_broadcaster_py_node_py")
        self.broadcaster = StaticTransformBroadcaster(self)
        self.pub_static_tf(argv)
    def pub_static_tf(self,argv):
        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = argv[7]
        ts.child_frame_id = argv[8]
        ts.transform.translation.x = float(argv[1])
        ts.transform.translation.y = float(argv[2])
        ts.transform.translation.z = float(argv[3])
        qtn = tf_transformations.quaternion_from_euler(
            float(argv[4]), 
            float(argv[5]), 
            float(argv[6])
        )
        ts.transform.rotation.x = qtn[0]
        ts.transform.rotation.y = qtn[1]
        ts.transform.rotation.z = qtn[2]
        ts.transform.rotation.w = qtn[3]
        self.broadcaster.sendTransform(ts)
def main():
    if len(sys.argv) != 9:
        get_logger("rclpy").error("非法传参")
        return
    rclpy.init()
    rclpy.spin(TFStaticBroadCasterPy(sys.argv))
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

动态广播

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

class TFDynaBroadcaster: public rclcpp::Node{
public:
  TFDynaBroadcaster():Node("tf_dyna_broadcaster_node_cpp"){
    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    pose_sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,
      std::bind(&TFDynaBroadcaster::do_pose,this,std::placeholders::_1)
    );
  }
private:
  void do_pose(const turtlesim::msg::Pose &pose)
  {
    geometry_msgs::msg::TransformStamped ts;
    
    ts.header.stamp = this->now();
    ts.header.frame_id = "world";
    ts.child_frame_id = "turtle1";
    ts.transform.translation.x = pose.x;
    ts.transform.translation.y = pose.y;
    ts.transform.translation.z = 0.0;
    
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose.theta);

    ts.transform.rotation.x = qtn.x();
    ts.transform.rotation.y = qtn.y();
    ts.transform.rotation.z = qtn.z();
    ts.transform.rotation.w = qtn.w();

    broadcaster_->sendTransform(ts);
  }
  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<TFDynaBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf_transformations
    
class TFDynaBroadcasterPy(Node):
    def __init__(self):
        super().__init__("tf_dyna_broadcaster_py_node_py")
        self.broadcaster = TransformBroadcaster(self)
        self.sub = self.create_subscription(
            Pose,
            "/turtle1/pose",
            self.do_pose,
            10)
    def do_pose(self,pose):
        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = "world"
        ts.child_frame_id = "turtle1"
        ts.transform.translation.x = pose.x
        ts.transform.translation.y = pose.y
        ts.transform.translation.z = 0.0
        qtn = tf_transformations.quaternion_from_euler(0.0,0.0,pose.theta)
        ts.transform.rotation.x = qtn[0]
        ts.transform.rotation.y = qtn[1]
        ts.transform.rotation.z = qtn[2]
        ts.transform.rotation.w = qtn[3]
        self.broadcaster.sendTransform(ts)

def main():
    rclpy.init()
    rclpy.spin(TFDynaBroadcasterPy())
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

坐标点发布

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

using namespace std::chrono_literals;

class PointBroadcaster: public rclcpp::Node{
public:
  PointBroadcaster():Node("point_broadcaster_node_cpp"),x(0.0){
    point_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>("point",10);
    timer_ = this->create_wall_timer(1s,std::bind(&PointBroadcaster::on_timer_,this));

  }
private:
  void on_timer_(){
    geometry_msgs::msg::PointStamped ps;
    ps.header.stamp = this->now();
    ps.header.frame_id = "laser";
    x += 0.05;
    ps.point.x = x;
    ps.point.y = 0.0;
    ps.point.z = -0.1;

    point_pub_->publish(ps);
  }
  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr point_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  double_t x;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<PointBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped
    
class PointBroadcasterPy(Node):
    def __init__(self):
        super().__init__("point_broadcaster_py_node_py")
        self.point_pub = self.create_publisher(
            PointStamped,
            "point",
            10)
        self.x = 0.2
        self.timer = self.create_timer(1.0,self.on_timer)
    def on_timer(self):
        ps = PointStamped()
        ps.header.stamp = self.get_clock().now().to_msg()
        ps.header.frame_id = "laser"
        self.x += 0.05
        ps.point.x = self.x
        ps.point.y = 0.0
        ps.point.z = 0.3
        self.point_pub.publish(ps)
def main():
    rclpy.init()
    rclpy.spin(PointBroadcasterPy())
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

坐标系变换监听

依赖rclcpp tf2 tf2_ros geometry_msgs

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
    
using namespace std::chrono_literals;

class TFListener: public rclcpp::Node{
public:
  TFListener():Node("tf_listener_node_cpp"){
    buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
    timer_ = this->create_wall_timer(1s,std::bind(&TFListener::on_timer_,this));
  }
private:
  void on_timer_(){

    try
    {
      auto ts = buffer_->lookupTransform("camera","laser",tf2::TimePointZero);
      RCLCPP_INFO(this->get_logger(),"------转换完成的坐标信息------");
      RCLCPP_INFO(this->get_logger(),
        "新坐标帧:父坐标系：%s,子坐标系：%s,偏移量(%.2f,%.2f,%.2f)",
        ts.header.frame_id.c_str(),
        ts.child_frame_id.c_str(),
        ts.transform.translation.x,
        ts.transform.translation.y,
        ts.transform.translation.z
      );
    }
    catch(const tf2::TransformException & e)
    {
      RCLCPP_ERROR(this->get_logger(),"转换异常: %s",e.what());
    }
    
  }
  std::shared_ptr<tf2_ros::Buffer> buffer_;
  std::shared_ptr<tf2_ros::TransformListener> listener_;
  rclcpp::TimerBase::SharedPtr timer_;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<TFListener>());
  rclcpp::shutdown();
  return 0;
}


import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from rclpy.time import Time

class TFListenerPy(Node):
    def __init__(self):
        super().__init__("tf_listener_py_node_py")
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # 判断是否可以实现转换
        if self.buffer.can_transform("camera", "laser", Time()):
            ts = self.buffer.lookup_transform("camera","laser",Time())
            self.get_logger().info("-----转换后数据-----")
            self.get_logger().info(
                "转换的结果,父坐标系：%s,子坐标系：%s,偏移量：(%.2f,%.2f,%.2f)"
                  % (ts.header.frame_id, ts.child_frame_id, 
                    ts.transform.translation.x, 
                    ts.transform.translation.y, 
                    ts.transform.translation.z
                    )
            )
        else:
            self.get_logger().error("转换失败......")

def main():
    rclpy.init()
    rclpy.spin(TFListenerPy())
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

坐标点变换监听

依赖rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs message_filters

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "message_filters/subscriber.h"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/message_filter.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
    
using namespace std::chrono_literals;

class TFPointListener: public rclcpp::Node{
public:
  TFPointListener():Node("tf_point_listener_node_cpp"){
    buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers,
    timer_ = std::make_shared<tf2_ros::CreateTimerROS>(
        this->get_node_base_interface(),
        this->get_node_timers_interface()
    );
    buffer_->setCreateTimerInterface(timer_);
    listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
    point_sub.subscribe(this, "point");
    /*
    F &f, 订阅对象
    tf2_ros::Buffer &buffer, 坐标缓存
    const std::string &target_frame, base_link
    uint32_t queue_size, 队列长度
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr &node_logging, 日志接口
    const rclcpp::node_interfaces::NodeClockInterface::SharedPtr &node_clock, 时钟接口
    std::chrono::duration<TimeRepT, TimeT> buffer_timeout 超时
    */
    filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
        point_sub,
        *buffer_,
        "base_link",
        10,
        this->get_node_logging_interface(),
        this->get_node_clock_interface(),
        1s
    );
    // 解析数据
    filter_->registerCallback(&TFPointListener::transform_point, this);
  }
private:
    void transform_point(const geometry_msgs::msg::PointStamped &ps){
        auto out = buffer_->transform(ps,"base_link");
        RCLCPP_INFO(this->get_logger(),"父级坐标系：%s,坐标：（%.2f,%.2f,%.2f)",
            out.header.frame_id.c_str(),
            out.point.x,
            out.point.y,
            out.point.z
        );
    }
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    std::shared_ptr<tf2_ros::CreateTimerROS> timer_;
    message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub;
    std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> filter_;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<TFPointListener>());
  rclcpp::shutdown();
  return 0;
}

坐标变换工具

1.tf2_monitor

打印所有坐标系的发布频率与网络延迟

ros2 run tf2_ros tf2_monitor

打印指定坐标系的发布频率与网络延迟

ros2 run tf2_ros tf2_monitor camera laser

2.tf2_echo

打印两个坐标系的平移旋转关系

ros2 run tf2_ros tf2_echo world turtle1

3.view_frames

以图形化的方式显示坐标系关系

ros2 run tf2_tools view_frames
I'm coming!!
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值