如何在ROS下編寫自己的節點來訂閱話題（C++）

本文暫定爲記錄，目前寫了一個訂閱話題的節點
–2015.10.13

一、首先建一個包

我的工作空間catkin_ws事先建好了，路徑是/home/siat/catkin_ws
然後運行以下命令在src文件夾下建立test包

$ cd ~/catkin_ws/src
$ catkin_create_pkg test roscpp

注意在包的名字後加上一些基本的依賴，比如roscpp和rospy這樣就可以直接調用C++和python節點了，這將會在CMakeLists.txt中出現

find_package(catkin REQUIRED COMPONENTS roscpp)

---

注意如果沒有加依賴，而是直接運行

$ catkin_create_pkg test

來創建包的，在CMakeLists.txt中是這樣的

find_package(catkin REQUIRED)

在後面編譯C++節點的時候將會提示“對各種ros類下的函數未定義的引用”

---

二、編寫節點文件

ROS裏每個節點都是一個執行文件，如果用C++來寫的話就是.cpp文件編譯生成的可執行文件，而Python就是.py文件。
在~/catkin_ws/src/test/src下新建一個test1.cpp文件作爲測試節點，如下

/*test1.cpp
*創建時間：2015.10.13
*   作者：張京林
*/
#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard odom:[%s]",msg->data.c_str());
    std::cout<<" chatterCallback run"<<std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test1");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("talker",1000,chatterCallback);
    std::cout<<" for test1"<<std::endl;
    ros::spin();

    return 0;
}

三、編寫CMakeLists.txt文件

這裏僅僅在自動生成的CMakeLists.txt文件後面加了以下兩句話，並沒有額外添加什麼依賴

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

上面第一句是生成可執行文件的，注意第二句不要漏掉，否則會報錯說是裏面的init,chatterCallback等函數未定義

四、編譯

$ cd ~/catkin_ws
$ catkin_make --pkg test

執行編譯完成後運行該節點就行了

$ rosrun test test1

test1節點訂閱了“talker”話題，我們可以找一個節點來測試一下，就拿官網編寫節點的教程裏的一個talker節點吧。這個節點名字叫talker但是它發佈的話題是“chatter”（見代碼73行），所以我把這個talker.cpp文件保存到~/catkin_ws/src/test/src下面作爲test包下的一個節點，還要修改代碼第73行：

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
改爲
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("talker", 1000);

然後在CMakeLists.txt後面加入兩句：

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

最後我的CMakeLists.txt長這樣：

cmake_minimum_required(VERSION 2.8.3)
project(test)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp)


catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test
#  CATKIN_DEPENDS other_catkin_pkg
#  DEPENDS system_lib
)


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

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

就行了，重新編譯一下test包：

$ cd ~/catkin_ws
$ catkin_make --pkg test

分別開兩個終端運行這兩個節點：

$ rosrun test talker

$ rosrun test test1

如圖：

五、訂閱其他話題遇到的問題

現在我知道消息訂閱節點的大體框架了，現在我想訂閱其他節點該怎麼改寫這段程序呢？
假設我要訂閱的節點是turtlesim包下面的turtlesim_node節點發布的話題turtle1/pose，那麼我該如何做？

（1）首先運行roscore和turtlesim_node，查看話題信息

$ roscore

$ rosrun turtlesim turtlesim_node

查看話題/turtle1/pose的類型

$ rostopic type /turtle1/pose

結果如下：

turtlesim/pose

繼續運行如下命令查看話題類型turtlesim/pose的詳細信息

$ rosmsg show /turtle1/pose

結果如下：

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

（2）其次查看相應的頭文件

查看http://docs.ros.org/api/turtlesim/html/index-msg.html中的Wiki page for turtlesim，轉到http://wiki.ros.org/turtlesim頁面，發現節點列表裏有如下內容：

2.1.2 Published Topics
turtleX/pose (turtlesim/Pose)
The x, y, theta, linear velocity, and angular velocity of turtleX.

進入turtlesim/Pose鏈接後有如下內容：

turtlesim/Pose Message

File: turtlesim/Pose.msg

Raw Message Definition

float32 x
float32 y
float32 theta

float32 linear_velocity
float32 angular_velocity
Compact Message Definition

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

autogenerated on Sun, 20 Sep 2015 23:15:36

所以要使用該話題的內容需要包含turtlesim/Pose.h頭文件
所以在文件包含的時候加入語句：

#include "turtlesim/Pose.h"

注：ROS各種API查詢
http://docs.ros.org/indigo/api/
http://docs.ros.org/api/

（3）編寫回調函數

因爲float32 x表明話題內容中的數據類型有float32類型，這是包內自己定義的類型，接收這些數據需要用到C++中的float類型變量，這個不需要額外添加頭文件包含；
在opt/ros/indigo/中查找turtlesim文件夾，找到其中的Pose.h，發現如下語句：

typedef boost::shared_ptr< ::turtlesim::Pose const> PoseConstPtr;

故而在回調函數的參數列表中消息類型應該寫const turtlesim::PoseConstPtr&

注：turtlesim/Pose.h是turtlesim/Pose.msg自動生成的頭文件，這類由.msg生成的消息頭文件一般名字都不變，後綴爲.h
且許多消息類型格式都是[包名]::[消息文件名][ConstPtr]&
比如：const turtlesim::PoseConstPtr&、
const geometry_msgs::TwistConstPtr&等

回調函數如下：

void chatterCallback_pose(const turtlesim::PoseConstPtr& msg)
{
    float x, y, theta;
    x=msg->x;
    y=msg->y;
    theta=msg->theta;
    ROS_INFO("I heard pose x:[%f] y:[%f] z:[%f]",x, y, theta);
}

這樣就可以取出話題/turtle1/pose中的數據x,y,theta用來運算了，以下是主要文件代碼：
test1.cpp

/*test1.cpp
*創建時間：2015.10.13
*   作者：張京林
*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "turtlesim/Pose.h"

//#include <sstream>

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard odom:[%s]",msg->data.c_str());
    std::cout<<" chatterCallback run"<<std::endl;
}

void chatterCallback_pose(const turtlesim::PoseConstPtr& msg)
{
    float x, y, theta;
    x=msg->x;
    y=msg->y;
    theta=msg->theta;
    ROS_INFO("I heard pose x:[%f] y:[%f] z:[%f]",x, y, theta);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test1");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("turtle1/pose",1000,chatterCallback_pose);
    //std::cout<<" for test1"<<std::endl;
    ros::spin();

    return 0;
}

CMakeLists.txt文件不用再做修改了，還是前面說的在自動生成的文件末尾加兩句話用來生成節點test1的可執行文件：

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

查看前面是否依賴了roscpp，如果沒有，在REQUIRED後面加上 COMPONENTS roscpp

find_package(catkin REQUIRED COMPONENTS roscpp)

修改完後編譯這個包：

$ cd catkin_ws
$ catkin_make --pkg test

（4）運行消息訂閱節點

先運行turtlesim包中的turtlesim_node節點：

$ rosrun turtlesim turtlesim_node

顯示/turtle1/pose話題：

$ rostopic echo /turtle1/pose

得到如下結果：

x: 5.544444561
y: 5.544444561
theta: 0.0
linear_velocity: 0.0
angular_velocity: 0.0
---

再運行test包中的test1節點：

$ rosrun test test1

結果如下：

[ INFO] [1444898679.863117000]: I heard pose x:[5.544445] y:[5.544445] z:[0.000000]