ROS study notes: TF coordinate transformation and programming exercises
For the basic concepts and implementation methods of TF, see the previous blog post: ROS study notes: TF coordinate transformation and programming details
In this article, we will perform TF programming exercises on the basis of the previous ones.
TF programming: broadcast and monitor the coordinate transformation of the robot, know the coordinate relationship between the lidar and the robot chassis, and find the coordinate value of the lidar data in the chassis coordinate system
We program in accordance with TF's use process:
TF broadcaster programming
Implementation process:
- Initialize the ROS node
- Define a broadcaster
- Create coordinate transformation Transform and initialize related information
- Broadcaster publishes conversion information
The specific implementation of the program is as follows:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv)
{
//初始化ROS节点
ros::init(argc, argv, "tf_broadcaster");
ros::NodeHandle n;
//声明一个tf广播器
tf::TransformBroadcaster br;
while(ros::ok())
{
//创建坐标变换transform
tf::Transform transform;
//给定平移变换值
transform.setOrigin(tf::Vector3(0.1, 0.0, 0.2));
//给定旋转变换值,这里没有旋转变换,所以定义四元数时无需给定角度
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
//广播器发布坐标变换
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));
}
return 0;
}
TF monitor programming
Implementation process:
- Initialize ROS node information
- Declare a TF listener
- Create a point on "base_laser" and get the corresponding value
- Monitor to obtain the transformation information, and convert the point on "base_laser" to get the coordinate point on "base_link"
Implementation details:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
int main(int argc, char** argv)
{
//初始化ROS节点信息
ros::init(argc, argv, "tf_listener");
ros::NodeHandle n;
//创建TF监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while(ros::ok())
{
//在base_laser上声明一个点laser_point,用来转换得到base_link上的点坐标
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "base_laser";
//使用最近可用的转换
laser_point.header.stamp = ros::Time();
//laser_point检测点获取
laser_point.point.x = 0.3;
laser_point.point.y = 0.0;
laser_point.point.z = 0.0;
try
{
//监听获取tf变换
listener.waitForTransform("base_link", "base_laser", ros::Time(0), ros::Duration(3.0));
geometry_msgs::PointStamped base_point;
base_point.header.frame_id = "base_link";
listener.transformPoint("base_link", laser_point, base_point);
//将坐标变换值进行打印
ROS_INFO("Change laser_point:(%.2f, %.2f, %.2f) to base_point(%.2f, %.2f, %.2f) at Time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
}
rate.sleep();
}
return 0;
}
The following figure is the content of the "geometry_msgs/PoseStamped.h" message, which declares 2 types: header and point
header internal
point internal
Configure the launch file
Then configure the CMakeLists.txt and package.xml files to execute.
Here is the result