ROS中话题的订阅与发布+ROS环境下上位机与Arduino单片机通讯

准备工作

创建工作空间

$ mkdir -p ～/catkin_ws/src
$ cd ～/catkin_ws/src
$ catkin_init_workspace

进入代码空间，使用catkin_create_pkg命令创建功能包：

$ cd ～/catkin_ws/src
$ catkin_create_pkg learning_communication std_msgs rospy roscpp

然后回到工作空间的根目录下进行编译，并且设置环境变量：

$ cd ～/catkin_ws
$ catkin_make
$ source ～/catkin_ws/devel/setup.bash

工作空间和功能包的创建这里就不过多重复讲了

创建Publisher

Publisher的主要作用是针对指定话题发布特定数据类型的消息。我们尝试使用代码实现一个节点，节点中创建一个Publisher并发布字符串“Hello World”。
在功能包（暂命名为learning——communication）的src文件夹里建立一个talker.cpp文件，内容为：
（此处注释比较详细，大家可以直接下载我整理好的功能包去运行：）

#include 
#include "ros/ros.h"
#include "std_msgs/String.h"
//为了避免包含繁杂的ROS功能包头文件，ros/ros.h已经帮我们包含了大部分ROS中通用的头文件。节点会发布String类型的消息，所以需要先包含该消息类型的头文件String.h。该头文件根据String.msg的消息结构定义自动生成，我们也可以自定义消息结构，并生成所需要的头文件。

int main(int argc, char **argv)
{
// ROS节点初始化：初始化ROS节点。该初始化的init函数包含三个参数，前两个参数是命令行或launch文件输入的参数，可以用来完成命名重映射等功能；第三个参数定义了Publisher节点的名称，而且该名称在运行的ROS中必须是独一无二的，不允许同时存在相同名称的两个节点。
ros::init(argc, argv, "talker");

// 创建节点句柄，，方便对节点资源的使用和管理。
ros::NodeHandle n;

// 创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::String，第二个参数表示消息发布队列的大小，当发布消息的实际速度较慢时，Publisher会将消息存储在一定空间的队列中；如果消息数量超过队列大小时，ROS会自动删除队列中最早入队的消息。
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// 设置循环的频率，单位是Hz，这里设置的是10 Hz。当调用Rate：sleep（）时，ROS节点会根据此处设置的频率休眠相应的时间，以保证循环维持一致的时间周期。
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
//进入节点的主循环，在节点未发生异常的情况下将一直在循环中运行，一旦发生异常，ros：ok（）就会返回false，跳出循环。

// 初始化std_msgs::String类型的消息，这里我们使用了最为简单的String消息类型，该消息类型只有一个成员，即data，用来存储字符串数据。
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();

// 发布封装完毕的消息msg。消息发布后，Master会查找订阅该话题的节点，并且帮助两个节点建立连接，完成消息的传输。
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);

// 循环等待回调函数
ros::spinOnce();

// 按照循环频率延时，现在Publisher一个周期的工作已经完成，可以让节点休息一段时间，调用休眠函数，节点进入休眠状态。当然，节点不可能一直休眠下去，别忘了之前设置了10Hz的休眠时间，节点休眠100ms后又会开始下一个周期的循环工作。
loop_rate.sleep();
++count;
}
return 0;
}

实现Publisher的流程可总结为：
初始化ROS节点→在ROS Master注册节点信息→按照一定的频率发布消息

创建Subscriber

创建一个Subscriber以订阅Publisher节点发布的“Hello World”字符串
在功能包的src文件夹里建立一个listener.cpp文件，内容为：
（和talker.cpp中重复的代码部分不再详细注释）

#include "ros/ros.h"
#include "std_msgs/String.h"

// 接收到订阅的消息后，会进入消息回调函数
//回调函数是订阅节点接收消息的基础机制，当有消息到达时会自动以消息指针作为参数，再调用回调函数，完成对消息内容的处理。如上是一个简单的回调函数，用来接收Publisher发布的String消息，并将消息数据打印出来。
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
// 将接收到的消息打印出来
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "listener");
// 创建节点句柄
ros::NodeHandle n;

// 创建一个Subscriber，订阅名为chatter的话题，注册回调函数chatterCallback
//订阅节点首先需要声明自己订阅的消息话题，该信息会在ROS Master中注册。Master会关注系统中是否存在发布该话题的节点，如果存在则会帮助两个节点建立连接，完成数据传输。
//NodeHandle：subscribe（），这里为n.subscribe,用来创建一个Subscriber。第一个参数即为消息话题；第二个参数是接收消息队列的大小，和发布节点的队列相似，当消息入队数量超过设置的队列大小时，会自动舍弃时间戳最早的消息；第三个参数是接收到话题消息后的回调函数。
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

// 循环等待回调函数，ros：spin（）在ros：ok（）返回false时退出。
ros::spin();
return 0;
}

实现Subscriber的流程可总结为：
初始化ROS节点→订阅需要的话题→循环等待话题消息，接收到消息后进入回调函数→回调函数中完成消息处理。
编译功能包
节点的代码已经完成，C++是一种编译语言，在运行之前需要将代码编译成可执行文件，如果使用Python等解析语言编写代码，则不需要进行编译，可以省去此步骤。
ROS中的编译器使用的是CMake，编译规则通过功能包中的CMakeLists.txt文件设置，使用catkin命令创建的功能包中会自动生成该文件，已经配置多数编译选项，并且包含详细的注释，我们几乎不用查看相关的说明手册，稍作修改就可以编译自己的代码。
打开功能包中的CMakeLists.txt文件，找到以下配置项，去掉注释并稍作修改：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker ${PROJECT_NAME}_generate_messages_cpp)
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(talker ${PROJECT_NAME}_generate_messages_cpp)

对于这个功能包，主要用到了以下四种编译配置项：
（1）include_directories
用于设置头文件的相对路径。全局路径默认是功能包的所在目录，比如功能包的头文件一般会放到功能包根目录下的include文件夹中，所以此处需要添加该文件夹。此外，该配置项还包含ROS catkin编译器默认包含的其他头文件路径，比如ROS默认安装路径、Linux系统路径等。
（2）add_executable
用于设置需要编译的代码和生成的可执行文件。第一个参数为期望生成的可执行文件的名称，后边的参数为参与编译的源码文件（cpp），如果需要多个代码文件，则可在后面依次列出，中间使用空格进行分隔。
（3）target_link_libraries
用于设置链接库。很多功能需要使用系统或者第三方的库函数，通过该选项可以配置执行文件链接的库文件，其第一个参数与add_executable相同，是可执行文件的名称，后面依次列出需要链接的库。此处编译的Publisher和Subscriber没有使用其他库，添加默认链接库即可。
（4）add_dependencies
用于设置依赖。在很多应用中，我们需要定义语言无关的消息类型，消息类型会在编译过程中产生相应语言的代码，如果编译的可执行文件依赖这些动态生成的代码，则需要使用add_dependencies添加${PROJECT_NAME}_generate_messages_cpp配置，即该功能包动态产生的消息代码。该编译规则也可以添加其他需要依赖的功能包。
以上编译内容会帮助系统生成两个可执行文件：talker和listener，放置在工作空间的～/catkin_ws/devel/lib/路径下。
CMakeLists.txt修改完成后，在工作空间的根路径下开始编译：

$ cd ～/catkin_ws
$ catkin_make

运行Publisher与Subscriber

首先要设置环境变量：

$ cd ～/catkin_ws
$ source ./devel/setup.bash

也可以将环境变量的配置脚本添加到终端的配置文件中：

$ echo "source ～/catkin_ws/devel/setup.bash" >> ～/.bashrc
$ source ～/.bashrc

设置好之后就可以进入启动步骤了：

1.请出ROS master

$ roscore

2．启动Publisher

$ rosrun learning_communication talker

3.启动Subscriber

$ rosrun learning_communication listener

/
//可以看到，消息已经实现同步的发布和订阅了
若关闭talker的话，会看待listener马上停止

下面说一下ROS下 上位机与Arduino单片机通讯

ROS下 上位机与Arduino单片机通讯

对于Arduino IDE的配置安装和串口通讯，请点这里
Publisher示例
主要就是如何通过rosserial创建publisher
例子来自Arduino IDE，File->Example->ros_lib下的HelloWord，下面给大家简单注释一下

#include 
#include  

//创建节点句柄
ros::NodeHandle  nh;

// 声明一个消息对象str_msg。其参数为data内容为消息内容。
std_msgs::String str_msg;

// 发布一个话题，名字叫做chatter，消息内容
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()
{
  nh.initNode();
  nh.advertise(chatter);
}

void loop()
{
  str_msg.data = hello;
  chatter.publish( &str_msg );//发布一个消息   话题.publish(&消息)
  nh.spinOnce();
  delay(1000);
}

接下来打开终端分别运行
1.把ros master请出来

roscore

2.新终端运行，/dev/ttyUSB0为Arduino设备

rosrun rosserial_python serial_node.py /dev/ttyUSB0

3.显示主题chatter，获取Arduino板反馈的信息

rostopic echo chatter

Subscriber示例
主要就是如何通过rosserial创建subscriber，点亮Arduino上的LED灯
例子来自Arduino IDE，File->Example->ros_lib下的blink，下面给大家简单注释一下

/* 
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */
//必需包含的ros头文件和消息头文件
#include 
#include 

ros::NodeHandle  nh;

//创建回调函数messageCb，必需传递常量消息引用值作为参数，这里函数是messageCb。
//消息类型是std_msgs::Empty，消息名称是toggle_msg
//在函数内，我们可以引用toggle_msg，但它是空的，就不必要了。
//Arduino每次收到信息就会点亮灯。
void messageCb( const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
}

//这里实例化订阅，有两个参数，主题名toggle_led和回调函数名，标明主题的消息类型std_msgs::Empty
ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()
{ 
  //初始化ROS节点处理，并宣告所有的发布或订阅
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{  
  //在Arduino的loop函数，调用nh.spinOnce()，这样所有的ROS回调函数就会被处理。
  nh.spinOnce();
  delay(1);
}

1.请出master

$ roscore

2.新终端运行，/dev/ttyUSB0为Arduino设备

$ rosrun rosserial_python serial_node.py /dev/ttyUSB0

3.发布主题，点亮Arduino板上的LED灯

$ rostopic pub toggle_led std_msgs/Empty --once