ROS 機器人技術 - 廣播與接收 TF 變換

上次我們學習了 TF 的基本概念和如何發(fā)布靜態(tài)的 TF 坐標：

• ROS 機器人技術 - TF 坐標系統(tǒng)基本概念
• ROS 機器人技術 - 靜態(tài) TF 坐標幀

這次來總結下如何發(fā)布一個自定義的 TF 坐標轉換，并監(jiān)聽這個變換。

一、編寫 TF 廣播者

進入上次創(chuàng)建的 learning_tf2 包中：

roscd?learning_tf2

在 src 下新建一個 turtle_tf2_broadcaster.cpp 文件，代碼如下：

#include?

//?存儲要發(fā)布的坐標變換
#include?

//?四元數(shù)
#include?

//?變換廣播者
#include?

//?烏龜?shù)奈蛔硕x
#include?

std::string?turtle_name;

void?poseCallback(const?turtlesim::PoseConstPtr&?msg)?
{
????//?創(chuàng)建?tf?廣播對象
????static?tf2_ros::TransformBroadcaster?br;

????//?存儲要發(fā)布的坐標變換消息
????geometry_msgs::TransformStamped?transformStamped;

????//?變換的時間戳
????transformStamped.header.stamp?=?ros::Time::now();
????
????//?父坐標系名稱
????transformStamped.header.frame_id?=?"world";
????
????//?當前要發(fā)布的坐標系名稱?-?烏龜?shù)拿?/span>
????transformStamped.child_frame_id?=?turtle_name;

????//?烏龜在二維平面運動，所以?z?坐標高度為?0
????transformStamped.transform.translation.x?=?msg->x;
????transformStamped.transform.translation.y?=?msg->y;
????transformStamped.transform.translation.z?=?0.0;

????//?用四元數(shù)存儲烏龜?shù)男D角
????tf2::Quaternion?q;

????//?因為烏龜在二維平面運動，只能繞?z?軸旋轉，所以?x，y?軸的旋轉量為?0
????q.setRPY(0,?0,?msg->theta);

????//?把四元數(shù)拷貝到要發(fā)布的坐標變換中
????transformStamped.transform.rotation.x?=?q.x();
????transformStamped.transform.rotation.y?=?q.y();
????transformStamped.transform.rotation.z?=?q.z();
????transformStamped.transform.rotation.w?=?q.w();
????
????//?用?tf?廣播者把訂閱的烏龜位姿發(fā)布到?tf?中
????br.sendTransform(transformStamped);
}

int?main(int?argc,?char**?argv)
{
????//?當前節(jié)點的名稱
????ros::init(argc,?argv,?"my_tf2_broadcaster");
????ros::NodeHandle?private_node("~");

????//?判斷當前要廣播的烏龜節(jié)點名字
????if?(!private_node.hasParam("turtle"))?{
????????//?launch?文件和命令行都沒有傳遞烏龜名稱，就直接退出
????????if?(argc?!=?2)?{
????????????ROS_ERROR("need?turtle?name?as?argument");
????????????return?-1;
????????};

????????//?launch?文件中如果沒有定義烏龜名稱，就在命令行中加上
????????turtle_name?=?argv[1];
????}?else?{
????????//?從?launch?文件獲取烏龜名稱參數(shù)
????????private_node.getParam("turtle",?turtle_name);
????}

????ros::NodeHandle?node;

????//?訂閱一個節(jié)點的?pose?msg，在回調(diào)函數(shù)中廣播訂閱的位姿消息到?tf2?坐標系統(tǒng)中
????//?turtle_name?為?turtle1?時廣播?turtle1?的位姿到?tf?中
????//?turtle_name?為?turtle2?時廣播?turtle2?的位姿到?tf?中
????ros::Subscriber?sub?=?node.subscribe(turtle_name?+?"/pose",?10,?&poseCallback);

????ros::spin();
????return?0;
};

這個程序的意思是訂閱輸入烏龜?shù)?pose 話題，然后在 poseCallback 回調(diào)函數(shù)中發(fā)布 world 到烏龜?shù)?TF 變換，注意這個程序可以接收不同烏龜?shù)?pose 消息，只要運行時指定烏龜?shù)拿Q turtle_name 即可，代碼注釋很詳細，其他的就不說了，然后添加編譯規(guī)則：

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
target_link_libraries(turtle_tf2_broadcaster ${catkin_LIBRARIES})

直接編譯：

catkin_make

基本上不會出問題，為了方便啟動我們在 launch 文件中啟動廣播者：

<launch>
?????
????<node?pkg="turtlesim"?type="turtlesim_node"?name="sim"/>

????
????<node?pkg="turtlesim"?type="turtle_teleop_key"?name="teleop"?output="screen"/>
????
????
????<param?name="scale_linear"?value="2"?type="double"/>
????<param?name="scale_angular"?value="2"?type="double"/>

????
????<node?pkg="learning_tf2"?type="turtle_tf2_broadcaster"?args="/turtle1"?name="turtle1_tf2_broadcaster"?/>
????
?????
????<node?pkg="learning_tf2"?type="turtle_tf2_broadcaster"?args="/turtle2"?name="turtle2_tf2_broadcaster"?/>

??launch>

然后就可以直接啟動了：

roslaunch?learning_tf2?start_demo.launch

為了確定是否成功廣播了變換，使用下面的命令查看一個變換的輸出：

rosrun?tf?tf_echo?/world?/turtle1

如果在控制臺輸出類似下面的消息，則說明變換發(fā)布成功：

下面我們來編寫一個 TF 接收者來使用我們上面發(fā)布的變換。

二、編寫 TF 接收者

同樣在 src 目錄下創(chuàng)建 turtle_tf2_listener.cpp，代碼如下：

#include?

//?接受?tf?變換
#include?

//?轉換消息?
#include?

//?發(fā)布到烏龜 2 的運動消息：角速度和線速度
#include?

//?再生服務
#include?

//?實現(xiàn)烏龜?2?跟隨烏龜?1?運動
int?main(int?argc,?char**?argv)
{
????//?當前節(jié)點的名字
????ros::init(argc,?argv,?"my_tf2_listener");

????ros::NodeHandle?node;
????ros::service::waitForService("spawn");
????ros::ServiceClient?spawner?=?node.serviceClient("spawn");
????
????turtlesim::Spawn?turtle;
????
????turtle.request.x?=?4;
????turtle.request.y?=?2;
????turtle.request.theta?=?0;
????turtle.request.name?=?"turtle2";
????spawner.call(turtle);

????//?角速度和線速度消息發(fā)布者，用來發(fā)布計算后的新的速度消息
????ros::Publisher?turtle_vel?=?node.advertise("turtle2/cmd_vel",?10);

????//?tf?變換緩存區(qū)，最多緩存?10?秒
????tf2_ros::Buffer?tfBuffer;

????//?創(chuàng)建監(jiān)聽?tf?變換對象，創(chuàng)建完畢即開始監(jiān)聽，通常定義為成員變量
????tf2_ros::TransformListener?tfListener(tfBuffer);
????
????ros::Rate?rate(10.0);
????while?(node.ok())?{
????????//?用來保存尋找的坐標變換
????????geometry_msgs::TransformStamped?transformStamped;
????????try{
???????????//?尋找坐標變換
??????????transformStamped?=?tfBuffer.lookupTransform("turtle2",?"turtle1",?ros::Time(0));
????????}
????????catch?(tf2::TransformException?&ex)?{
????????????ROS_WARN("%s",ex.what());
????????????ros::Duration(1.0).sleep();
????????????continue;
????????}

????????//?用來保存角速度和線速度
????????geometry_msgs::Twist?vel_msg;

????????//?新的角速度為尋找到的變換角速度的?4?倍?-?使得第二個烏龜?shù)倪\動軌跡轉彎更快，且軌跡是弧線
????????vel_msg.angular.z?=?4.0?*?atan2(transformStamped.transform.translation.y,?transformStamped.transform.translation.x);
????????
????????//?新的線速度是尋找到的變換線速度的?0.5?倍?-?使得第二個烏龜?shù)倪\動速度為第一個烏龜?shù)囊话?/span>
????????vel_msg.linear.x?=?0.5?*?sqrt(pow(transformStamped.transform.translation.x,?2)?+?pow(transformStamped.transform.translation.y,?2));
????????
????????//?發(fā)布新的速度消息，烏龜?2?節(jié)點的內(nèi)部訂閱了這個消息，所以烏龜?2?會收到新的角速度和線速度，以此產(chǎn)生跟隨運動
????????turtle_vel.publish(vel_msg);
??????
????????rate.sleep();
????}

????return?0;
};

這里關鍵的代碼如下：

//?保存尋找的變換
geometry_msgs::TransformStamped?transformStamped;

//?尋找?turtle1?到?turtle2?的坐標變換
//?target_frame:?turtle2?
//?source_frame:?turtle1
//?ros::Time(0):?獲取變換的時間，
transformStamped?=?tfBuffer.lookupTransform("turtle2",?"turtle1",?ros::Time(0));

同樣添加編譯規(guī)則：

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES})

然后編譯：

catkin_make

在上面廣播者的 launch 文件中加上接收者的啟動：

<launch>
?????
????<node?pkg="turtlesim"?type="turtlesim_node"?name="sim"/>

????
????<node?pkg="turtlesim"?type="turtle_teleop_key"?name="teleop"?output="screen"/>
????
????
????<param?name="scale_linear"?value="2"?type="double"/>
????<param?name="scale_angular"?value="2"?type="double"/>

????
????<node?pkg="learning_tf2"?type="turtle_tf2_broadcaster"?args="/turtle1"?name="turtle1_tf2_broadcaster"?/>
????
?????
????<node?pkg="learning_tf2"?type="turtle_tf2_broadcaster"?args="/turtle2"?name="turtle2_tf2_broadcaster"?/>

????
????<node?pkg="learning_tf2"?type="turtle_tf2_listener"?name="listener"?/>

??launch>

然后啟動：

roslaunch?learning_tf2?start_demo.launch

運行時會出現(xiàn) 2 個小烏龜，把窗口焦點放到終端，按上下左右鍵會發(fā)現(xiàn)第二個烏龜跟隨第一個烏龜運動：

但是剛啟動時終端會報個錯誤：

[ERROR]?[1418082761.220546623]:?"turtle2"?passed?to?lookupTransform?argument?target_frame?does?not?exist.
[ERROR]?[1418082761.320422000]:?"turtle2"?passed?to?lookupTransform?argument?target_frame?does?not?exist.

這是因為我們在 turtle2 還沒有產(chǎn)生之前就尋找變換，導致沒有找到它，為了解決這個問題可以在尋找變換前等待變換可用：

//?第四個參數(shù)是阻塞等待的超時時間
listener.waitForTransform("/turtle2",?"/turtle1",?ros::Time::now(),?ros::Duration(3.0));

transformStamped?=?tfBuffer.lookupTransform("turtle2",?"turtle1",?ros::Time(0));

加上這句運行時就不會報錯了，今天就寫到這里，下次見：）