ROS的空間描述和變換

1、tf的實際應用

1）在機器人的配置中

從上面可以看出激光雷達中心距離機器人底座的中心有20cm，激光雷達的中心距機器人底座中心有10cm，如果激光雷達在障礙物前面0.3米，那麼機器人底座離障礙物多遠呢?

1、新建一個包，如下

catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

2、catkin_make一下

3、新建兩個源文件tf_publisher.cpp,tf_listener.cpp

下面按照base_link和base_laser的位置來編寫publisher

#include<ros/ros.h>

#include<tf/transform_broadcaster.h>

int main(int argc,char **argv)

{

　　ros::init(argc,argv,"tf_publisher");

　　ros::NodeHandle n;

　　//發佈頻率

　　ros::Rate r(100);

　　//創建一個transform_broadcaster

　　tf::TransformBroadcaster broadcaster;

　　//創建一個transform對象

　　tf::Transform transform;

　　//下面設置其位置以及旋轉信息

　　transform.setOrigin(tf::Vector3(0.1,0.0,0.2));

　　//再創建一個四元組

　　tf::Quaternion q;

　　q.setRPY(0.0,0.0,0.0);//沒有旋轉

　　transform.setRotation(q);//以上已經把位置描述設定好了

　　while(ros::ok())

　　{

　　　　broadcaster.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","base_laser"));//父框架叫base_link,子框架叫base_laser

　　　　r.sleep();

　　}

　　return 0;

}

下面在tf_listener.cpp當中來編寫

在其中使用了一個新的類型叫做geometry_msgs/PointStamped

在命令行當中查看該類型rosmsg show geometry_msgs/PointStamped

#include<ros/ros.h>

#include<geometry_msgs/PointStamped.h>

#include<tf/transform_listener.h>

void transformPoint(const tf::TransformListener &listener)

{

　　//實例化一個消息

　　geometry_msgs::PointStamped laser_point;

　　//指定header的frame_id,表示最新的可用的變換

　　laser_point.header.framer_id="base_laser";

　　laser_point.header.stamp = ros::Time();

　　laser_point.point.x= rand()%5;;//賦予一個隨機值

　　laser_point.point.y= rand()%5;;//賦予一個隨機值

　　laser_point.point.z= rand()%5;;//賦予一個隨機值

　　try

　　{

　　　　geometry_msgs::PointStamped base_point;//實例化一個geometry_msgs::PointStamped類，表示機器人底座

　　　　//調用transformlistener的transformpoint函數

　　　　listener.transformPoint("base_link",laser_point,base_point);//參數目標框架target_frame,stamped_in以及stamped_out。

　　　　//輸出相關信息

　　　　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 \"base_laser\" to \"base_link\": %s", ex.what());
  }

}

int main(int argc,char **argv)

{

　　ros::init(argc,argv,"tf_listener");

　　ros::NodeHandle n;

　　//定義一個transformlistener對象

　　tf::TransformListener listener(ros::Duration(10))//等待10s，如果10s之後都還沒收到消息，那麼之前的消息就被丟棄掉。

　　//創建一個timer對象，綁定了一個transformpoint的地址，後面是該函數transformpoint的參數

　　ros::Timer timer=n.createrTimer(ros::Duration(1.0),boost::bind(&transformPoint,boost::ref(listener)));

　　//在上面去實現transformPoint這個函數

 ros::spin();

}

對上面代碼總結一下：

在main函數實例化一個listener對象，然後創建一個定時器1s鐘調用上面函數一次，…。

下面把CMakeLists.txt修改一下：

add_executable(tf_listener src/tf_listener.cpp);

add_executable(tf_publisher src/tf_publisher.cpp);

target_link_libraries(tf_listener ${catkin_LIBRARIES})

target_link_libraries(tf_publisher ${catkin_LIBRARIES})

再寫一個launch文件：

<launch>

//再添加一個frame稱爲camera把它固定在base_link上，用static_transform_publisher來指定base_link（父框架）和camera（子框架）的關係，

//位置信息0.1 0.0 0.2，方向信息爲0 0 0 1

<node pkg="tf" type="static_transform_publisher" name="broadcaster" args="0.1 0.0 0.2 0 0 0 1 base_link camera 100">

//我們要啓動的node

<node pkg="robot_setup_tf" type="tf_publisher" name="publisher"/>

<node pkg="robot_setup_tf" type="tf_listener" name="listener"/>

</launch>

下面去編譯：

catkin_make

運行之：

roscore

rosrun robot_setup_tf tf_listener

rosrun robot_setup_tf tf_publisher

然後進入rviz中去觀察。

然後再看看launch文件：

roslaunch robot_setup_tf demo.launch    //在launch文件中我們又指定了一個座標系框架叫做camera

下面去看一下

rosrun tf view_frames

生成一個PDF文件，可以看到：