ros tf_tutorial的修改，跟随方式从turtle/c
昨天困扰个问题到今天，希望要让turtle2的坐标永远相对于turtle1的坐标都是静止的，turtle2由turtle1平移而来。本来是根据tf_tutorial来做的，结果发现有个问题就是，那个turtle会一直原地绕某个点按照一定的角速度旋转
阅读下面之前建议读者先照着上面的tutorial做一遍
方案1：a.新建的carrot1（由turtle1平移而来）
b.新建一个broadcaster发布carrot1相对于turtle1的位置
c.listener让turtle2向carrot1移动
但由于“/turtle/cmd_vel”消息类型是geometry_msgs::Twist，只能从transform中获取线速度和角速度进行平移。
这里要注意一点，我最开始犯的错误就是只建了三个frame（请忽略右边的point！）
image.png
我建了三个坐标系world，turtle1，turtle2，然后用两个broadcaster分别发布了world中turtle1的位置和turtle1中turtle2的位置，在listener中直接listener.lookupTransform("/world","/turtle2",ros::Time(0), transform);
这种直接按照tutorial修改而来的方式，运行结果并不能达到要求，因为前面提到了，控制turtle2是用线速度和角速度的，很明显上面获取到的tranform转换而来线速度和角速度的话，会让turtle2一直在原地旋转
方案2: 不使用消息turtle2/cmd_vel（类型为geometry_msgs::Twist，具体包含就是线速度和角速度），查阅turtlesim文档，发现有service为teleport_absolute，使用这个服务而不是使用线速度和角速度对turtle2进行控制，很遗憾的是关于teleport_absolute的例子很少，上面那个wiki界面本来是有视频教程的由于是youtube所以看不了（当然有能力翻墙的小伙伴还是看看为好，我只改了host只能用用google）
a.同样的，两个广播发布world中turtle1的位置和turtle1中turtle2的位置
b.listener中同样lookuptransform找出world中turtle2的坐标转换transform，从其中取出x，y然后用teleport_absolute直接指定turtle2的位置
代码如下
turtle_tf_broadcaster.cpp 和tutorial里面一模一样，作用就是发布turtle1在world中的位置
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
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);
ros::Time now = ros::Time::now();
br.sendTransform(tf::StampedTransform(transform, 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;
//订阅turtle1/pose消息，然后调用回掉函数发布turtle1相对world的位置关系
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
turtle12_tf_broadcaster.cpp 发布turtle2在turtle1坐标系中的位置
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
//transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/turtle1", "/turtle2")); //parent,son
rate.sleep();
}
return 0;
}
最后这个文件是重点，turtle_tf_listener.cpp 监听turtle2在world中的位置，利用teleport_absolute进行变换
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <turtlesim/TeleportAbsolute.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;
//设定turtle2的初始值，这里设置的其实没用，因为运行起来之后下面的teleport会直接把turtle2平移到之前发布的turtle1和turtle2相对的位置去，这里留着做个备忘
srv.request.x = 4;
srv.request.y = 1;
srv.request.theta = 0;
//利用spawn创建新的turtle
add_turtle.call(srv);
//改用teleport_absolute 服务的方式控制turtle2的移动
ros::service::waitForService("turtle2/teleport_absolute");
ros::ServiceClient teleport_turtle =
node.serviceClient<turtlesim::TeleportAbsolute>("turtle2/teleport_absolute");
turtlesim::TeleportAbsolute telesrv;
//tutorial中使用的是这种方式，在本例中弃用这种方式
//ros::Publisher turtle_vel =
// node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);
tf::TransformListener listener;
static tf::TransformBroadcaster br;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
//直接查找world中turtle2的位置，最后一个参数是设置超时
//这里其实还有个问题，之前tutorial里面有个time travel的章节，让turtle2跟随turtle1五秒或者几秒之前的坐标
//但是实际我试验的情况是，如果我的turtle1停着不动，最终turtle2还是会和turtle1重合，这一点我猜测是/turtle1/pose消息发送的频率问题，它是一直在发送的
//如果time travel有成功的小伙伴可以告知我一声
listener.waitForTransform("/world","/turtle2",ros::Time(0),ros::Duration(10.0) );
//lookupTransform的意思就是找出从world变换到turtle2的变换方式(可以理解为turtle2在world坐标系中的位置)存入transform中
listener.lookupTransform("/world","/turtle2",ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
telesrv.request.x = transform.getOrigin().x();
telesrv.request.y = transform.getOrigin().y();
telesrv.request.theta = 0.0;
teleport_turtle.call(telesrv);
}
return 0;
};
tf相关工具
- 用rosrun rqt_tf_tree rqt_tf_tree查看各个frames之间的树状关系 本例如上面那个图
- 用rosrun tf tf_echo /point //turtle1 查找turtle1相对于point的运动变换（注：point是固定在world中的任意一点，就是创建一个broadcaster发布point相对world的位置）
- 用rosrun rqt_graph rqt_graph查看消息的来龙去脉
- 用
rosrun rviz rviz -d `rospack find learning_tf`/rviz/tule_rviz.rviz查看坐标在rviz下面的变换 learning_tf记得替换为你直接的包名
这个工具可能会出现Fixed Frame [map] does not exist找不到map frame的情况，开着rviz的情况下再起一个命令行，运行
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map world 10world为你的父frame名
