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

创建工作空间

$ mkdir -p &＃xff5e;/catkin_ws/src
$ cd &＃xff5e;/catkin_ws/src
$ catkin_init_workspace

进入代码空间&＃xff0c;使用catkin_create_pkg命令创建功能包&＃xff1a;

$ cd &＃xff5e;/catkin_ws/src
$ catkin_create_pkg learning_communication std_msgs rospy roscpp

然后回到工作空间的根目录下进行编译&＃xff0c;并且设置环境变量&＃xff1a;

$ cd &＃xff5e;/catkin_ws
$ catkin_make
$ source &＃xff5e;/catkin_ws/devel/setup.bash

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

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

#include
#include "ros/ros.h"
#include "std_msgs/String.h"
//为了避免包含繁杂的ROS功能包头文件&＃xff0c;ros/ros.h已经帮我们包含了大部分ROS中通用的头文件。节点会发布String类型的消息&＃xff0c;所以需要先包含该消息类型的头文件String.h。该头文件根据String.msg的消息结构定义自动生成&＃xff0c;我们也可以自定义消息结构&＃xff0c;并生成所需要的头文件。int main(int argc, char **argv)
{
// ROS节点初始化&＃xff1a;初始化ROS节点。该初始化的init函数包含三个参数&＃xff0c;前两个参数是命令行或launch文件输入的参数&＃xff0c;可以用来完成命名重映射等功能&＃xff1b;第三个参数定义了Publisher节点的名称&＃xff0c;而且该名称在运行的ROS中必须是独一无二的&＃xff0c;不允许同时存在相同名称的两个节点。
ros::init(argc, argv, "talker");// 创建节点句柄&＃xff0c;&＃xff0c;方便对节点资源的使用和管理。
ros::NodeHandle n;// 创建一个Publisher&＃xff0c;发布名为chatter的topic&＃xff0c;消息类型为std_msgs::String&＃xff0c;第二个参数表示消息发布队列的大小&＃xff0c;当发布消息的实际速度较慢时&＃xff0c;Publisher会将消息存储在一定空间的队列中&＃xff1b;如果消息数量超过队列大小时&＃xff0c;ROS会自动删除队列中最早入队的消息。
ros::Publisher chatter_pub &＃61; n.advertise<std_msgs::String>("chatter", 1000);
// 设置循环的频率&＃xff0c;单位是Hz&＃xff0c;这里设置的是10 Hz。当调用Rate&＃xff1a;sleep&＃xff08;&＃xff09;时&＃xff0c;ROS节点会根据此处设置的频率休眠相应的时间&＃xff0c;以保证循环维持一致的时间周期。
ros::Rate loop_rate(10);
int count &＃61; 0;
while (ros::ok())
{
//进入节点的主循环&＃xff0c;在节点未发生异常的情况下将一直在循环中运行&＃xff0c;一旦发生异常&＃xff0c;ros&＃xff1a;ok&＃xff08;&＃xff09;就会返回false&＃xff0c;跳出循环。// 初始化std_msgs::String类型的消息&＃xff0c;这里我们使用了最为简单的String消息类型&＃xff0c;该消息类型只有一个成员&＃xff0c;即data&＃xff0c;用来存储字符串数据。
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data &＃61; ss.str();// 发布封装完毕的消息msg。消息发布后&＃xff0c;Master会查找订阅该话题的节点&＃xff0c;并且帮助两个节点建立连接&＃xff0c;完成消息的传输。
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);// 循环等待回调函数
ros::spinOnce();// 按照循环频率延时&＃xff0c;现在Publisher一个周期的工作已经完成&＃xff0c;可以让节点休息一段时间&＃xff0c;调用休眠函数&＃xff0c;节点进入休眠状态。当然&＃xff0c;节点不可能一直休眠下去&＃xff0c;别忘了之前设置了10Hz的休眠时间&＃xff0c;节点休眠100ms后又会开始下一个周期的循环工作。
loop_rate.sleep();
&＃43;&＃43;count;
}
return 0;
}

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

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

#include "ros/ros.h"
#include "std_msgs/String.h"// 接收到订阅的消息后&＃xff0c;会进入消息回调函数
//回调函数是订阅节点接收消息的基础机制&＃xff0c;当有消息到达时会自动以消息指针作为参数&＃xff0c;再调用回调函数&＃xff0c;完成对消息内容的处理。如上是一个简单的回调函数&＃xff0c;用来接收Publisher发布的String消息&＃xff0c;并将消息数据打印出来。
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&＃xff0c;订阅名为chatter的话题&＃xff0c;注册回调函数chatterCallback
//订阅节点首先需要声明自己订阅的消息话题&＃xff0c;该信息会在ROS Master中注册。Master会关注系统中是否存在发布该话题的节点&＃xff0c;如果存在则会帮助两个节点建立连接&＃xff0c;完成数据传输。
//NodeHandle&＃xff1a;subscribe&＃xff08;&＃xff09;&＃xff0c;这里为n.subscribe,用来创建一个Subscriber。第一个参数即为消息话题&＃xff1b;第二个参数是接收消息队列的大小&＃xff0c;和发布节点的队列相似&＃xff0c;当消息入队数量超过设置的队列大小时&＃xff0c;会自动舍弃时间戳最早的消息&＃xff1b;第三个参数是接收到话题消息后的回调函数。
ros::Subscriber sub &＃61; n.subscribe("chatter", 1000, chatterCallback);// 循环等待回调函数&＃xff0c;ros&＃xff1a;spin&＃xff08;&＃xff09;在ros&＃xff1a;ok&＃xff08;&＃xff09;返回false时退出。
ros::spin();
return 0;
}

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

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)

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

$ cd &＃xff5e;/catkin_ws
$ catkin_make
运行Publisher与Subscriber

首先要设置环境变量&＃xff1a;

$ cd &＃xff5e;/catkin_ws
$ source ./devel/setup.bash

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

$ echo "source &＃xff5e;/catkin_ws/devel/setup.bash" >> &＃xff5e;/.bashrc
$ source &＃xff5e;/.bashrc

设置好之后就可以进入启动步骤了&＃xff1a;

1.请出ROS master

$ roscore

2&＃xff0e;启动Publisher

$ rosrun learning_communication talker

3.启动Subscriber

$ rosrun learning_communication listener

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

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

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

#include
#include //创建节点句柄
ros::NodeHandle nh;// 声明一个消息对象str_msg。其参数为data内容为消息内容。
std_msgs::String str_msg;// 发布一个话题&＃xff0c;名字叫做chatter&＃xff0c;消息内容
ros::Publisher chatter("chatter", &str_msg);char hello[13] &＃61; "hello world!";void setup()
{nh.initNode();nh.advertise(chatter);
}void loop()
{str_msg.data &＃61; hello;chatter.publish( &str_msg );//发布一个消息 话题.publish(&消息)nh.spinOnce();delay(1000);
}

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

roscore

2.新终端运行&＃xff0c;/dev/ttyUSB0为Arduino设备

rosrun rosserial_python serial_node.py /dev/ttyUSB0

3.显示主题chatter&＃xff0c;获取Arduino板反馈的信息

rostopic echo chatter

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

/* * rosserial Subscriber Example* Blinks an LED on callback*/
//必需包含的ros头文件和消息头文件
#include
#include ros::NodeHandle nh;//创建回调函数messageCb&＃xff0c;必需传递常量消息引用值作为参数&＃xff0c;这里函数是messageCb。
//消息类型是std_msgs::Empty&＃xff0c;消息名称是toggle_msg
//在函数内&＃xff0c;我们可以引用toggle_msg&＃xff0c;但它是空的&＃xff0c;就不必要了。
//Arduino每次收到信息就会点亮灯。
void messageCb( const std_msgs::Empty& toggle_msg){digitalWrite(13, HIGH-digitalRead(13)); // blink the led
}//这里实例化订阅&＃xff0c;有两个参数&＃xff0c;主题名toggle_led和回调函数名&＃xff0c;标明主题的消息类型std_msgs::Empty
ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );void setup()
{ //初始化ROS节点处理&＃xff0c;并宣告所有的发布或订阅pinMode(13, OUTPUT);nh.initNode();nh.subscribe(sub);
}void loop()
{ //在Arduino的loop函数&＃xff0c;调用nh.spinOnce()&＃xff0c;这样所有的ROS回调函数就会被处理。nh.spinOnce();delay(1);
}

1.请出master

$ roscore

2.新终端运行&＃xff0c;/dev/ttyUSB0为Arduino设备

$ rosrun rosserial_python serial_node.py /dev/ttyUSB0


3.发布主题&＃xff0c;点亮Arduino板上的LED灯

$ rostopic pub toggle_led std_msgs/Empty --once