1. Coordinate transformation in the robot
The TF function package is used to manage all coordinate systems. It can record the relationship between all coordinate systems within ten seconds, and can show the position of the gripped object relative to the center coordinate system of the robot.
2. Example: Little turtle following experiment
1. Little turtles follow
After the two turtles appear, one turtle is at the center point and the other turtle appears below. The turtle in the center can be controlled to move, and the turtle below will automatically follow the turtle we control to move.
sudo apt-get install ros-noetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
#rosrun turtlesim turtle_teleop_key
Among them, roslaunch is used to start script files and start many of
them. Noetic is the ROS version number.
Press the arrow keys in the terminal to control the tortoise being followed.
If there is an error in the noetic version of ubuntu20.04, you can refer to the following method to solve it
cd /usr/bin sudo rm -r python # 有的可能没有这个文件，就省略这一步 sudo cp python3 python
2. View tf relationship
rosrun tf view_frames
Wait for 5 seconds to generate a pdf file, open it to see the positional relationship of tf coordinates in the current system.
The world is the global coordinate system, and the other turtle1 and turtle2 are the coordinate systems on the two turtles. The purpose of the routine is to make the two coordinate systems overlap in coordinates.
If there is an error in this step, you need to modify the file that reported the error. Just add a sentence above
sudo gedit /opt/ros/noetic/lib/tf/view_frames
line 88 .print(vstr)vstr=str(vstr)
3. tf_echo coordinate relationship
rosrun tf tf_echo turtle1 turtle2
Output the relationship between the two coordinate systems, describing how the turtle2 coordinate system is transformed to the turtle1 coordinate system. Including Translation translation and Rotation rotation (quaternion, radian, angle three ways to describe rotation).
4. rviz 3D visualization display platform
rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz
First change the Fixed Frame on the left to world
Click Add at the bottom left to add a TF to display the positional relationship of TF
Control the movement of the turtle, you can see that the two coordinate systems in the picture are moving
3. Programming realization of TF coordinate system broadcasting and monitoring
1. Create a feature package
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
2. Create tf broadcaster code
Open the learning_tf/src/catalog and create aturtle_tf_broadcaster.cpp
Its content is:
/**
* 该例程产生tf数据，并计算、发布turtle2的速度指令
* REFERENCE:www.guyuehome.com
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster br;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
};
3. Create the listener listener code
Similarly, create another one turtle_tf_listener.cppwith the content
/**
* 该例程监听tf数据，并计算、发布turtle2的速度指令
* REFERENCE:www.guyuehome.com
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");
// 创建节点句柄
ros::NodeHandle node;
// 请求产生turtle2
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
4. Configure tf broadcaster and listener code compilation rules
In the configuration learning_tf, CMakeLists.txtadd the following code in the position of the figure
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
That is, the two cpp files are compiled into two executable files, and then the library is linked.
5. Compile
cd ~/catkin_ws
catkin_make
6. Run the program
Each line of the following program requires a separate terminal to run.
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key
