文章目录
- 目标功能
- 工程文件框架
- 乌龟仿真节点和键盘控制节点
- tf广播节点
- tf监听节点
目标功能
- 在tf教程中,我们需要创建四种节点,分别是键盘控制节点
teleop
、乌龟仿真节点turtlesim
、tf广播节点tf broadcaster
、tf监听节点tf listener
。 -
turtlesim
启动后会默认生成一只小乌龟“turtle1”,tf listener
启动时负责在仿真器中生成另一只小乌龟“turtle2”。 -
teleop
发布turtle1/command_velocity
话题,turtlesim
订阅后“turtle1”开始运动。 -
turtlesim
发布/turtle1/pose
和/turtle2/pose
话题,tf broadcaster
订阅后转换成tf::StampedTransform
格式,得到“turtle1”和“turtle2”各自的坐标系,并进行广播。 -
tf listener
监听到坐标系广播后,根据“turtle1”和“turtle2”之间的变换关系,计算出“turtle2”跟随“turtle1”所需要的线速度和角速度,并发布turtle2/command_velocity
话题,turtlesim
订阅后,“turtle2”开始跟随运动。
工程文件框架
假设名为ros_ws的文件夹用来存放ROS程序
- ros_ws
- build
- devel
- src
- learning_tf
- include
- launch
- start_demo.launch
- src
- turtle_tf_broadcaster.cpp
- turtle_tf_listener.cpp
- CMakeLists.txt
- package.xml
- CMakeLists.txt
乌龟仿真节点和键盘控制节点
在launch文件中设置需要启动的节点以及所需参数
<launch>
<!-- Turtlesim节点-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
</launch>
sim节点启动后,仿真器中会出现一个随机外形的小乌龟,默认命名为“turtle1”。turtlesim
这个包提供了spawn
服务用来生成小乌龟,命名规则是按生成的序号,如“turtle2”、“turtle3”。默认键盘控制第一个小乌龟“turtle1”。
tf广播节点
turtle_tf_broadcaster.cpp源代码为:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
/*
* @ brief 接收到小乌龟的位姿数据后,转换成Transoform格式并广播
* @ param msg 从turtlesim订阅的乌龟位姿
*/
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
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);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
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;
};
每次从话题/turtlex/pose
(这里x等于1或2)得到小乌龟二维位姿的数据,就转换为一个关于三维位姿的变换矩阵(从世界坐标系“world”变换到乌龟坐标系“turtlex”),然后发布出去。
注意到sendTransform
的第二个参数,是指发布该变换矩阵的时间,这里用的是系统的当前时间。
为了生成可执行文件并顺利启动,在CMakeLists.txt中添加:
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
在launch文件中添加:
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
tf监听节点
turtle_tf_listener.cpp源代码为:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel = node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
tf::TransformListener listener;
ros::Rate rate(10.0); //发布频率为10Hz,即每100ms发布一次
while (node.ok()){
/* 监听turtle2到turtle1的变换 */
tf::StampedTransform transform;
try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
/* 将tf::StampedTransform transform格式转换为turtlesim::Velocity格式并发布 */
turtlesim::Velocity 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;
};
前半部分是调用了turtlesim
包提供的spawn
服务来创建新的乌龟,后半部分的主循环有两个任务:
- 获取两只乌龟最新的以变换矩阵形式存在的位姿信息,计算从“turtle2”到“turtle1”的变换矩阵。
- 计算“turtle2”为了跟上“turtle1”所需要的线速度和角速度,并以
turtlesim::Velocity
的数据格式在发布"turtle2/command_velocity"
话题。
为了生成可执行文件并顺利启动,在CMakeLists.txt中添加:
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
在launch文件中添加:
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />