turtlesim用teleport做坐标变换,不用spawn和角速度做坐标变换
2017-07-04 11:39
411 查看
昨天困扰个问题到今天,希望要让turtle2的坐标永远相对于turtle1的坐标都是向某个地方平移的。本来是根据tf_tutorial来做的,建议读者先照着上面的tutorial做一遍
方案1:a.新建的carrot1(由turtle1平移而来)
b.新建一个broadcaster发布carrot1相对于turtle1的位置
c.listener让turtle2向carrot1移动
但由于“/turtle/cmd_vel”消息类型是geometry_msgs::Twist,只能从transform中获取线速度和角速度进行平移。
这里要注意一点,我最开始犯的错误就是只建了三个frame(请忽略左边的point!)
我建了三个坐标系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];
//turtle_name = "turtle2";
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
#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>
void transformPoint(const tf::TransformListener& listener){
/*we'll create a point in the base_laser frame that we'd like to transform
*to the base_link frame*/
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "turtle2";
//we'll just use the most recent transform available for our simple example
laser_point.header.stamp = ros::Time();
//just an arbitrary point in space
laser_point.point.x = 0.0;
laser_point.point.y = 2.0;
laser_point.point.z = 0.0;
try{
geometry_msgs::PointStamped base_point;
listener.transformPoint("turtle1", laser_point, base_point);
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.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("Received an exception trying to transform a point from \"turtle1\" to \"turtle2\": %s", ex.what());
}
}
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消息发送的频率问题,它是一直在发送的
listener.waitForTransform("/world","/turtle2",ros::Time(0),ros::Duration(10.0) );
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相关工具
1.用rosrun rqt_tf_tree rqt_tf_tree 查看各个frames之间的树状关系 本例如图
2.用rosrun tf tf_echo /point //turtle1 查找turtle1相对于point的运动变换(注:point是固定在world中的任意一点,就是创建一个broadcaster发布point相对world的位置)
3.用rosrun rqt_graph rqt_graph查看消息的来龙去脉
4.用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 10 。。。world为你
a395
的父frame名
方案1:a.新建的carrot1(由turtle1平移而来)
b.新建一个broadcaster发布carrot1相对于turtle1的位置
c.listener让turtle2向carrot1移动
但由于“/turtle/cmd_vel”消息类型是geometry_msgs::Twist,只能从transform中获取线速度和角速度进行平移。
这里要注意一点,我最开始犯的错误就是只建了三个frame(请忽略左边的point!)
我建了三个坐标系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];
//turtle_name = "turtle2";
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
#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>
void transformPoint(const tf::TransformListener& listener){
/*we'll create a point in the base_laser frame that we'd like to transform
*to the base_link frame*/
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "turtle2";
//we'll just use the most recent transform available for our simple example
laser_point.header.stamp = ros::Time();
//just an arbitrary point in space
laser_point.point.x = 0.0;
laser_point.point.y = 2.0;
laser_point.point.z = 0.0;
try{
geometry_msgs::PointStamped base_point;
listener.transformPoint("turtle1", laser_point, base_point);
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.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("Received an exception trying to transform a point from \"turtle1\" to \"turtle2\": %s", ex.what());
}
}
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消息发送的频率问题,它是一直在发送的
listener.waitForTransform("/world","/turtle2",ros::Time(0),ros::Duration(10.0) );
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相关工具
1.用rosrun rqt_tf_tree rqt_tf_tree 查看各个frames之间的树状关系 本例如图
2.用rosrun tf tf_echo /point //turtle1 查找turtle1相对于point的运动变换(注:point是固定在world中的任意一点,就是创建一个broadcaster发布point相对world的位置)
3.用rosrun rqt_graph rqt_graph查看消息的来龙去脉
4.用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 10 。。。world为你
a395
的父frame名
相关文章推荐
- Android中计算坐标变换速度的原理
- 【2015-2016 ACM-ICPC Pacific Northwest Regional Contest (Div 1)G】【坐标轴变换 LIS】Racing Gems 开车吃宝石,横向速度不能超
- [WebGL入门]十三,minMatrix.js和坐标变换矩阵
- 基于VC++的OpenGL编程讲座之坐标变换
- Opengl渲染管线 坐标变换 纹理与光照
- OpenGL坐标变换专题
- IOS中二维坐标变换
- ROS中的坐标变换
- 几大坐标变换
- 读HTML5应用开发与实践【四】【坐标变换】
- ROS之tf空间坐标变换浅析
- iPhone 横屏竖屏旋转时坐标原点变换的方法
- HTML 5中SVG 2D坐标与变换
- MATLAB 根据X、Y坐标绘制折线图,增加右侧纵轴刻度且不用plotyy
- Android OpenGL ES 简明开发教程四:3D 坐标变换
- 坐标系、坐标参照系、坐标变换、投影变换
- 基于VC++的OpenGL编程讲座之坐标变换(2)
- 附录A 8. Direct3D中的3D坐标变换
- LogPolar 坐标变换
- 矩阵的坐标变换(转)