ROS學習筆記四：用C++編寫ROS發布與訂閱

一、創建並編譯功能包

1.1 創建功能包

在工作空間的 src 目錄下創建功能包：

$ cd ~/dev/catkin_ws/src
$ catkin_create_pkg chapter2_tutorials std_msgs roscpp

1.2 編譯功能包

進入工作目錄下編譯全部功能包：

$ cd ~/dev/catkin_ws/
$ catkin_make

如果到達100%，表示一切順利，否則會報錯停止編譯。

二、創建並編譯節點

2.1 創建節點

進入功能包的 src 目錄下，在這個文件夾中，創建兩個新文件：example1_a.cpp 和 example1_b.cpp。

example1_a.cpp（發布節點）：

#include "ros/ros.h"                     //"ros/ros.h"包含ROS節點所有節點的必要文件
#include "std_msgs/String.h"             //"std_msgs/String.h"包含消息類型
#include <sstream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "example1_a"); //啟動該節點並設置其名稱（example1_a），該名稱是唯一的
    ros::NodeHandle n;                   //設置節點進程的句柄
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("message", 1000); //把這個節點設置成發布者，並把發布主題的類型告訴節點管理器。第一個參數是消息名稱“message”，第二個參數將緩沖區設置為1000個消息
    ros::Rate loop_rate(10);             //設置頻率10Hz
    
    while (ros::ok())                    //一直運行，直到CTRL+C停止運行
    {
        std_msgs::String msg;            //創建消息變量，變量類型必須符合發送的要求
        std::stringstream ss;
        ss << " I am the example1_a node "; //要發布的消息內容
        msg.data = ss.str();
        chatter_pub.publish(msg);        //發布消息
        ros::spinOnce();                 //如果出現訂閱者，ROS會更新和讀取所有主題
        loop_rate.sleep();               //按頻率掛起
    }
    
    return 0;
}

example1_b.cpp（訂閱節點）：

#include "ros/ros.h"
#include "std_msgs/String.h"
 
/*接收消息然后發布*/
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard: [%s]", msg->data.c_str());  //在命令行窗口顯示消息內容
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "example1_b");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("message", 1000, chatterCallback); //創建一個訂閱者，從主題獲取以“message”為名稱的消息，緩沖區為1000，處理消息句柄的回調函數chatterCallback
    ros::spin();  //ros::spin()庫是響應循環，消息到達時調用函數chatterCallback，CTRL+C結束循環
    return 0;
}

2.2 編譯節點

將下列幾行代碼添加到CMakeLists.txt文件中include_directories(include ${catkin_INCLUDE_DIRS})之后：

add_executable(chap2_example1_a src/example1_a.cpp)
add_executable(chap2_example1_b src/example1_b.cpp)
add_dependencies(chap2_example1_a chapter2_tutorials_generate_messages_cpp)
add_dependencies(chap2_example1_b chapter2_tutorials_generate_messages_cpp)
target_link_libraries(chap2_example1_a ${catkin_LIBRARIES})
target_link_libraries(chap2_example1_b ${catkin_LIBRARIES})

回到工作目錄下，編譯功能包：

$ cd ~/dev/catkin_ws/
$ catkin_make chapter2_tutorials

如果順利編譯的話，將會生成兩個可執行程序 example1_a 和 example1_b 文件，默認生成可執行文件的目錄是catkin工作空間的devel目錄下的lib目錄。

3 檢驗編寫的節點

首先要確認在carkin_make之后加載了工作空間的環境變量，然后分別在三個新的終端運行如下命令：

$ roscore
$ rosrun chapter2_tutorials chap2_example1_a
$ rosrun chapter2_tutorials chap2_example1_b

使用rqt_graph命令能夠創建一個顯示當前系統運行情況的動態圖形，如下圖所示。example1_a 節點發布 /message 主題，同時 example1_b 節點訂閱了這個主題。