ROS2学习笔记2
目录
ROS2学习笔记2
前言
本篇文章属于ROS2humble的学习笔记，来源于B站 up主。下面是这位up主的视频链接。本文为个人学习笔记，只能做参考，细节方面建议观看视频，肯定受益匪浅。
一、坐标变换工具TF
1. Python中的手眼变换坐标变换
（1）静态TF
import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster # 静态坐标发布器
from geometry_msgs.msg import TransformStamped # 消息接口
from tf_transformations import quaternion_from_euler # 欧拉角转四元数
import math # 角度转弧度函数
class StaticTFBroadcaster(Node):
def __init__(self):
super().__init__('static_tf_broadcaster')
self.static_broadcaster_ = StaticTransformBroadcaster(self)
self.publish_static_tf()
def publish_static_tf(self):
"""
发布静态TF 从 base_link 到 camera_link 之间的坐标关系
"""
transform = TransformStamped()
transform.header.frame_id = 'base_link'
transform.child_frame_id = 'camera_link'
transform.header.stamp = self.get_clock().now().to_msg()
transform.transform.translation.x = 0.5
transform.transform.translation.y = 0.3
transform.transform.translation.z = 0.6
q = quaternion_from_euler(math.radians(180),0,0)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.static_broadcaster_.sendTransform(transform)
self.get_logger().info(f'发布静态TF:{transform}')
def main():
rclpy.init()
node = StaticTFBroadcaster()
rclpy.spin(node)
rclpy.shutdown()（2）动态TF
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster # 坐标发布器
from geometry_msgs.msg import TransformStamped # 消息接口
from tf_transformations import quaternion_from_euler # 欧拉角转四元数
import math # 角度转弧度函数
class TFBroadcaster(Node):
def __init__(self):
super().__init__('tf_broadcaster')
self.broadcaster_ = TransformBroadcaster(self)
self.timer_ = self.create_timer(0.01,self.publish_tf)
def publish_tf(self):
"""
发布动态TF 从 camera_link 到 bottle_link 之间的坐标关系
"""
transform = TransformStamped()
transform.header.frame_id = 'camera_link'
transform.child_frame_id = 'bottle_link'
transform.header.stamp = self.get_clock().now().to_msg()
transform.transform.translation.x = 0.2
transform.transform.translation.y = 0.3
transform.transform.translation.z = 0.5
q = quaternion_from_euler(0,0,0)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.broadcaster_.sendTransform(transform)
self.get_logger().info(f'发布静态TF:{transform}')
def main():
rclpy.init()
node = TFBroadcaster()
rclpy.spin(node)
rclpy.shutdown()（3）查询TF关系
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener,Buffer # 坐标监听器
from tf_transformations import euler_from_quaternion # 四元数转欧拉角
import math # 角度转弧度函数
import rclpy.time
class TFListener(Node):
def __init__(self):
super().__init__('tf_listener')
self.buffer_ = Buffer()
self.listener_ = TransformListener(self.buffer_,self)
self.timer_ = self.create_timer(1.0,self.get_transform)
def get_transform(self):
"""
实时查询坐标关系
"""
try:
result = self.buffer_.lookup_transform('base_link','bottle_link',
rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))
transform = result.transform
self.get_logger().info(f'平移:{transform.translation}')
self.get_logger().info(f'旋转:{transform.rotation}')
rotation_euler = euler_from_quaternion([
transform.rotation.x,
transform.rotation.y,
transform.rotation.z,
transform.rotation.w,
])
self.get_logger().info(f'旋转RPY:{rotation_euler}')
except Exception as e:
self.get_logger().warn(f'获取坐标变换失败,原因:{str(e)}')
def main():
rclpy.init()
node = TFListener()
rclpy.spin(node)
rclpy.shutdown()2. C++中的地图坐标变换
（1）静态TF
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#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("static_tf_broadcaster")
{
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 q;
q.setRPY(0.0,0.0,60*M_PI/180.0);
transform.transform.rotation = tf2::toMsg(q);
this->broadcaster_->sendTransform(transform);
}
};
int main(int argc,char* argv[])
{
rclcpp::init(argc,argv);
auto node = std::make_shared<StaticTFBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
}（2）动态TF
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "chrono"
using namespace std::chrono_literals;
class TFBroadcaster:public rclcpp::Node
{
private:
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
rclcpp::TimerBase::SharedPtr timer_;
public:
TFBroadcaster() : Node("tf_broadcaster")
{
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
// this->publish_tf();
timer_ = this->create_wall_timer(100ms,std::bind(&TFBroadcaster::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 q;
q.setRPY(0.0,0.0,30*M_PI/180.0);
transform.transform.rotation = tf2::toMsg(q);
this->broadcaster_->sendTransform(transform);
}
};
int main(int argc,char* argv[])
{
rclcpp::init(argc,argv);
auto node = std::make_shared<TFBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
}（3）查询TF关系
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/utils.h"
#include "chrono"
using namespace std::chrono_literals;
class TFListener:public rclcpp::Node
{
private:
std::shared_ptr<tf2_ros::TransformListener> listener_;
std::shared_ptr<tf2_ros::Buffer> buffer_;
rclcpp::TimerBase::SharedPtr timer_;
public:
TFListener() : Node("tf_listener")
{
this->buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
this->listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
// this->publish_tf();
timer_ = this->create_wall_timer(1s,std::bind(&TFListener::get_tf,this));
}
void get_tf()
{
// 到Buffer查询坐标关系
try
{
const auto transform = buffer_->lookupTransform("base_link","target_point",this->get_clock()->now(),rclcpp::Duration::from_seconds(1.0f));
auto translation = transform.transform.translation;
auto rotation = transform.transform.rotation;
double y,p,r;
tf2::getEulerYPR(rotation,y,p,r);
RCLCPP_INFO(get_logger(),"平移:%f,%f,%f",translation.x,translation.y,translation.z);
RCLCPP_INFO(get_logger(),"旋转:%f,%f,%f",y,p,r);
}
catch(const std::exception& e)
{
RCLCPP_WARN(get_logger(),"%s",e.what());
}
}
// 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 q;
// q.setRPY(0.0,0.0,30*M_PI/180.0);
// transform.transform.rotation = tf2::toMsg(q);
// this->broadcaster_->sendTransform(transform);
// }
};
int main(int argc,char* argv[])
{
rclcpp::init(argc,argv);
auto node = std::make_shared<TFListener>();
rclcpp::spin(node);
rclcpp::shutdown();
}