这个是ROS2机器人的核心工具之一。
概念：
TF（坐标系转换）子系统是ROS2机器人框架中的一个重要组件，它的功能是提供坐标系转换服务，使得不同坐标系之间的数据可以转换。比如，机器人的传感器可以产生的数据是基于机器人本体坐标系的，而机器人的末端机械臂可以根据这些数据来控制机械臂的运动，但是机械臂需要的数据是基于机械臂坐标系的，这时候TF子系统就可以把数据从机器人本体坐标系转换成机械臂坐标系，从而实现机械臂的控制。
应用：
ROS2机器人TF子系统是一种基于消息传递的机器人框架，它可以帮助机器人开发者和研究人员更轻松、更快速地开发机器人系统。TF子系统可以帮助机器人开发者实现机器人定位、导航和路径规划等功能。比如，一个机器人可以使用TF子系统来实现室内定位，可以使用TF子系统来实现自主导航，也可以使用TF子系统来实现复杂的路径规划。
坐标变换消息：
ros2 interface show tf2_msgs/msg/TFMessage
geometry_msgs/TransformStamped[] transforms
例如机器人turtlebot、tiago等。
使用：
ros2 run rqt_tf_tree rqt_tf_tree
如果是类人机器人：
注意，如果没有指令，需要安装rqt-tf-tree功能包。
husky
为何选择这个案例呢？
机器人编程基础-ETH ROS 2022-Programming for Robotics_zhangrelay的博客-CSDN博客
2021（noetic）：
2020及之前（indigo/kinetic/melodic）：
书中所有案例均可以用husky和turtlebot3等复现。
典型案例：
odom →base footprint→ detected obstacle
其中base footprint→ detected obstacle：
-
geometry_msgs::msg::TransformStamped detection_tf;
-
detection_tf.header.frame_id =
"base_footprint";
-
detection_tf.header.stamp =
now();
-
detection_tf.child_frame_id =
"detected_obstacle";
-
detection_tf.transform.translation.x =
1.0;
-
tf_broadcaster_->
sendTransform(detection_tf);
odom →base footprint→ detected obstacle：
-
tf2_ros::Buffer tfBuffer;
-
tf2_ros::TransformListener tfListener(tfBuffer);
-
...
-
geometry_msgs::msg::TransformStamped odom2obstacle;
-
odom2obstacle = tfBuffer_.
lookupTransform(
"odom",
"detected_obstacle", tf2::TimePointZero);
ROS2机器人的tf2::TimePointZero是一个用来表示时间的类，它使用“秒”和“纳秒”来表示时间，可以用来跟踪和比较时间点。它可以用来构建时间戳，并且可以与其他时间类型（例如ros::Time）进行转换。
C++案例：
-
#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;
-
}
发布：
-
#include <functional>
-
#include <memory>
-
#include <sstream>
-
#include <string>
-
-
#include "geometry_msgs/msg/transform_stamped.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
#include "tf2/LinearMath/Quaternion.h"
-
#include "tf2_ros/transform_broadcaster.h"
-
#include "turtlesim/msg/pose.hpp"
-
-
class
FramePublisher :
public rclcpp::Node
-
{
-
public:
-
FramePublisher()
-
:
Node(
"turtle_tf2_frame_publisher")
-
{
-
// Declare and acquire `turtlename` parameter
-
turtlename_ =
this->
declare_parameter<std::string>(
"turtlename",
"turtle");
-
-
// Initialize the transform broadcaster
-
tf_broadcaster_ =
-
std::
make_unique<tf2_ros::TransformBroadcaster>(*
this);
-
-
// Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
-
// callback function on each message
-
std::ostringstream stream;
-
stream <<
"/" << turtlename_.
c_str() <<
"/pose";
-
std::string topic_name = stream.
str();
-
-
subscription_ =
this->
create_subscription<turtlesim::msg::Pose>(
-
topic_name,
10,
-
std::
bind(&FramePublisher::handle_turtle_pose,
this, std::placeholders::_1));
-
}
-
-
private:
-
void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
-
{
-
geometry_msgs::msg::TransformStamped t;
-
-
// Read message content and assign it to
-
// corresponding tf variables
-
t.header.stamp =
this->
get_clock()->
now();
-
t.header.frame_id =
"world";
-
t.child_frame_id = turtlename_.
c_str();
-
-
// Turtle only exists in 2D, thus we get x and y translation
-
// coordinates from the message and set the z coordinate to 0
-
t.transform.translation.x = msg->x;
-
t.transform.translation.y = msg->y;
-
t.transform.translation.z =
0.0;
-
-
// For the same reason, turtle can only rotate around one axis
-
// and this why we set rotation in x and y to 0 and obtain
-
// rotation in z axis from the message
-
tf2::Quaternion q;
-
q.
setRPY(
0,
0, msg->theta);
-
t.transform.rotation.x = q.
x();
-
t.transform.rotation.y = q.
y();
-
t.transform.rotation.z = q.
z();
-
t.transform.rotation.w = q.
w();
-
-
// Send the transformation
-
tf_broadcaster_->
sendTransform(t);
-
}
-
-
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
-
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-
std::string turtlename_;
-
};
-
-
int main(int argc, char * argv[])
-
{
-
rclcpp::
init(argc, argv);
-
rclcpp::
spin(std::
make_shared<FramePublisher>());
-
rclcpp::
shutdown();
-
return
0;
-
}
订阅：
-
#include <chrono>
-
#include <functional>
-
#include <memory>
-
#include <string>
-
-
#include "geometry_msgs/msg/transform_stamped.hpp"
-
#include "geometry_msgs/msg/twist.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
#include "tf2/exceptions.h"
-
#include "tf2_ros/transform_listener.h"
-
#include "tf2_ros/buffer.h"
-
#include "turtlesim/srv/spawn.hpp"
-
-
using
namespace std::chrono_literals;
-
-
class
FrameListener :
public rclcpp::Node
-
{
-
public:
-
FrameListener()
-
:
Node(
"turtle_tf2_frame_listener"),
-
turtle_spawning_service_ready_(
false),
-
turtle_spawned_(
false)
-
{
-
// Declare and acquire `target_frame` parameter
-
target_frame_ =
this->
declare_parameter<std::string>(
"target_frame",
"turtle1");
-
-
tf_buffer_ =
-
std::
make_unique<tf2_ros::Buffer>(
this->
get_clock());
-
tf_listener_ =
-
std::
make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-
-
// Create a client to spawn a turtle
-
spawner_ =
-
this->
create_client<turtlesim::srv::Spawn>(
"spawn");
-
-
// Create turtle2 velocity publisher
-
publisher_ =
-
this->
create_publisher<geometry_msgs::msg::Twist>(
"turtle2/cmd_vel",
1);
-
-
// Call on_timer function every second
-
timer_ =
this->
create_wall_timer(
-
1s, std::
bind(&FrameListener::on_timer,
this));
-
}
-
-
private:
-
void on_timer()
-
{
-
// Store frame names in variables that will be used to
-
// compute transformations
-
std::string fromFrameRel = target_frame_.
c_str();
-
std::string toFrameRel =
"turtle2";
-
-
if (turtle_spawning_service_ready_) {
-
if (turtle_spawned_) {
-
geometry_msgs::msg::TransformStamped t;
-
-
// Look up for the transformation between target_frame and turtle2 frames
-
// and send velocity commands for turtle2 to reach target_frame
-
try {
-
t = tf_buffer_->
lookupTransform(
-
toFrameRel, fromFrameRel,
-
tf2::TimePointZero);
-
}
catch (
const tf2::TransformException & ex) {
-
RCLCPP_INFO(
-
this->
get_logger(),
"Could not transform %s to %s: %s",
-
toFrameRel.
c_str(), fromFrameRel.
c_str(), ex.
what());
-
return;
-
}
-
-
geometry_msgs::msg::Twist msg;
-
-
static
const
double scaleRotationRate =
1.0;
-
msg.angular.z = scaleRotationRate *
atan2(
-
t.transform.translation.y,
-
t.transform.translation.x);
-
-
static
const
double scaleForwardSpeed =
0.5;
-
msg.linear.x = scaleForwardSpeed *
sqrt(
-
pow(t.transform.translation.x,
2) +
-
pow(t.transform.translation.y,
2));
-
-
publisher_->
publish(msg);
-
}
else {
-
RCLCPP_INFO(
this->
get_logger(),
"Successfully spawned");
-
turtle_spawned_ =
true;
-
}
-
}
else {
-
// Check if the service is ready
-
if (spawner_->
service_is_ready()) {
-
// Initialize request with turtle name and coordinates
-
// Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
-
auto request = std::
make_shared<turtlesim::srv::Spawn::Request>();
-
request->x =
4.0;
-
request->y =
2.0;
-
request->theta =
0.0;
-
request->name =
"turtle2";
-
-
// Call request
-
using ServiceResponseFuture =
-
rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
-
auto response_received_callback = [
this](ServiceResponseFuture future) {
-
auto result = future.
get();
-
if (
strcmp(result->name.
c_str(),
"turtle2") ==
0) {
-
turtle_spawning_service_ready_ =
true;
-
}
else {
-
RCLCPP_ERROR(
this->
get_logger(),
"Service callback result mismatch");
-
}
-
};
-
auto result = spawner_->
async_send_request(request, response_received_callback);
-
}
else {
-
RCLCPP_INFO(
this->
get_logger(),
"Service is not ready");
-
}
-
}
-
}
-
-
// Boolean values to store the information
-
// if the service for spawning turtle is available
-
bool turtle_spawning_service_ready_;
-
// if the turtle was successfully spawned
-
bool turtle_spawned_;
-
rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{
nullptr};
-
rclcpp::TimerBase::SharedPtr timer_{
nullptr};
-
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{
nullptr};
-
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{
nullptr};
-
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-
std::string target_frame_;
-
};
-
-
int main(int argc, char * argv[])
-
{
-
rclcpp::
init(argc, argv);
-
rclcpp::
spin(std::
make_shared<FrameListener>());
-
rclcpp::
shutdown();
-
return
0;
-
}
时间：
-
transformStamped = tf_buffer_->
lookupTransform(
-
toFrameRel,
-
fromFrameRel,
-
tf2::TimePointZero);
时间旅行：
-
rclcpp::Time when =
this->
get_clock()->
now() - rclcpp::
Duration(
5,
0);
-
t = tf_buffer_->
lookupTransform(
-
toFrameRel,
-
fromFrameRel,
-
when,
-
50ms);
转载：https://blog.csdn.net/ZhangRelay/article/details/128933531
查看评论