0
点赞
收藏
分享

微信扫一扫

ROS发布tf坐标


我们写个小程序来发布一个坐标系:

坐标系消息格式:

std_msgs/Header header                     #头信息
uint32 seq #|-- 序列号
time stamp #|-- 时间戳
string frame_id #|-- 坐标 ID
string child_frame_id #子坐标系的 id
geometry_msgs/Transform transform #坐标信息
geometry_msgs/Vector3 translation #偏移量
float64 x #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w

用下面的代码发布出来: 

#include "ros/ros.h"
#include <vector>
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"


int main(int argc, char** argv)
{
ros::init(argc, argv, "ik_test_node");
ros::NodeHandle n;
tf2_ros::StaticTransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.seq=100;
ts.header.stamp=ros::Time::now();
ts.header.frame_id="base_link";
ts.child_frame_id="laser";
ts.transform.translation.x=0.0;
ts.transform.translation.y=0.0;
ts.transform.translation.z=0.0;
ts.transform.rotation.x=0;
ts.transform.rotation.y=0;
ts.transform.rotation.z=0;
ROS_INFO("nihao woshi :%lf",ts.transform.rotation.x);
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();

broadcaster.sendTransform(ts);
ros::spin();
}

 运行程序,然后打开rviz载入坐标系,就可以看到我们发布的坐标系了:

ROS发布tf坐标_#include

如下图: 

ROS发布tf坐标_偏移量_02

发布一个动态的laser坐标系:

#include "ros/ros.h"
#include <vector>
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"


int main(int argc, char** argv)
{
ros::init(argc, argv, "ik_test_node");
ros::NodeHandle n;
tf2_ros::StaticTransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.seq=100;
ts.header.stamp=ros::Time::now();
ts.header.frame_id="base_link";
ts.child_frame_id="laser";
ts.transform.translation.x=0.2;
ts.transform.translation.y=0.0;
ts.transform.translation.z=0.5;
ts.transform.rotation.x=0;
ts.transform.rotation.y=0;
ts.transform.rotation.z=0;
ROS_INFO("nihao woshi :%lf",ts.transform.rotation.x);
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();
ros::Rate r(1);
while(ros::ok())
{
ts.transform.translation.x+=0.2;
broadcaster.sendTransform(ts);
r.sleep();
}
broadcaster.sendTransform(ts);
ros::spin();
}

订阅方将点的坐标从laser坐标系转换到base_link坐标系:

#include <ros/ros.h>
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"


int main(int argc,char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"tf_sub");
ros::NodeHandle nh;
tf2_ros::Buffer buffer;

tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);

while(ros::ok())
{
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id="laser";
point_laser.header.stamp=ros::Time::now();
point_laser.point.x=1;
point_laser.point.y=2;
point_laser.point.z=7.3;

try
{
geometry_msgs::PointStamped point_base;
point_base=buffer.transform(point_laser,"base_link");
ROS_INFO("zhuanhuanhoude shuju x:%f,y:%f,z:%f",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
ROS_INFO("chengxu yichang");
}

r.sleep();
ros::spinOnce();
}





}

打开rviz,加载坐标系,就可以看到这两个坐标系了,这次这个laser坐标系逐渐远离base_link。 

动态坐标转换:

订阅小乌龟位置和方向,然后发布出来。

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose)
{
static tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::TransformStamped ts;
ts.header.frame_id="world";
ts.header.stamp=ros::Time::now();
ts.child_frame_id="turtle1";
ts.transform.translation.x=pose->x;
ts.transform.translation.y=pose->y;
ts.transform.translation.z=0;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
ts.transform.rotation.x=qtn.getX();
ts.transform.rotation.y=qtn.getY();
ts.transform.rotation.z=qtn.getZ();
ts.transform.rotation.w=qtn.getW();
broadcaster.sendTransform(ts);
ROS_INFO("JINLAILE");

}

int main(int argc, char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_tf_pub");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
ros::spin();
return 0;
}

 当运行turtlesim节点和键盘控制节点时,控制小乌龟在界面上运动的时候,

通过rviz可以看到小乌龟身上的坐标系在跑:

ROS发布tf坐标_偏移量_03

参考链接:

​​http://www.autolabor.com.cn/book/ROSTutorials/di-5-zhang-ji-qi-ren-dao-hang/51-tfzuo-biao-bian-huan/511-zuo-biao-msg-xiao-xi.html​​


举报

相关推荐

0 条评论