如何在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的详细信息

$ rostopic 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]