ROS is the abbreviation of Robot Operating System. This article introduces the introduction of ROS application development, programming of TF coordinate system broadcasting and monitoring. There are 2 turtles in the little turtle simulation. The keyboard controls the first turtle to move around, and both turtles broadcast their coordinates. The monitor listens to the coordinates of the two turtles, obtains the relative position, and controls the second turtle to keep following the first turtle.
Project package creation
The ROS workspace is established in the article ROS Development and Application Preparation: Create a Workspace . Now, create a functional package:
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
The first line is to return to the src directory of the workspace. The function package creation must be run in this directory.
The second line is the command to create the function package, the first parameter is the name of the function package, here is learning_tf, and the next is the dependency library of the function package, here is a total of 4 dependency libraries of roscpp rospy tf turtlesim.
After the above command is executed, you can see a directory named learning_tf in the src directory with the following content:
CMakeLists.txt include package.xml src
Broadcaster code
In the src directory of the project directory, which is also the ~/catkin_ws/src/learning_tf/src directory, create a file turtle_tf_broadcaster.cpp,
cd ~//catkin_ws/src/learning_tf/src
nano turtle_tf_broadcaster.cpp
The content is:
/**
* 该例程产生tf数据，并计算、发布turtle2的速度指令
*/
#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;
};There are clear comments in the code
Listener code
In the src directory of the project directory, which is also the ~/catkin_ws/src/learning_tf/src directory, create a file turtle_tf_listener.cpp,
cd ~//catkin_ws/src/learning_tf/src
nano turtle_tf_listener.cpp
The content is:
/**
* 该例程监听tf数据，并计算、发布turtle2的速度指令
*/
#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;
};There are clear comments in the code
Compile configuration
In the ~/catkin_ws/src/learning_tf/ directory, there is a CMakeLists.txt file, we need to modify this file
cd ~/catkin_ws/src/learning_tf/
nano CMakeLists.txt
Add the following lines to this file,
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})
The added position is the end of the Build section. Refer to the following position, which is in front of ## install ##,
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
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})
#############
## Install ##
#############
Save, exit
This completes the compilation and configuration.
Test verification
Compilation must go back to the ~/catkin_ws directory
cd ~/catkin_ws
catkin_make
It should be sourced once after compilation:
source devel/setup.bash
If there is an error in the compilation, you must eliminate the error, and then run the test.
The test is more complicated and requires 6 terminals to be opened. And run the following commands in each terminal in turn:
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
After running the above instructions, control the keyboard direction keys on the keyboard control terminal to control the movement of the first turtle, and you can see that the second turtle keeps following the movement.
The source code can also be downloaded at https://github.com/huchunxu/ros_21_tutorials
That's it for the introduction.