ROS学习-写一个简单的Publisher和Subscriber

ROS学习-写一个简单的Publisher和Subscriber 写一个Publisher节点

“Node”是ROS中对可执行文件的一个术语，连接成ROS的网络。这里，我们将创建一个Publisher节点（talker）来持续地广播消息message。
首先，切换到beginner_tutorials包所在的位置。

roscd beginner_tutorials

创建一个src文件夹，存放源码。

mkdir -p src

Publisher节点源码

在src文件夹下创建一个talker.cpp源文件：

touch src/talker.cpp 

然后在talker.cpp源文件中粘贴下述内容：

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

#include 


int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "talker");

  
  ros::NodeHandle n;

  
  ros::Publisher chatter_pub = n.advertise("chatter", 1000);

  ros::Rate loop_rate(10);

  
  int count = 0;
  while (ros::ok())
  {
    
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    
    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

Publisher节点源码解释

下面我们对上述源码展开进行一个解释：
首先是：

#include "ros/ros.h"

ros/ros.h是一个方便的include头文件，其中包括ROS系统中所有头文件所必须的最常用的公共部分。
接下来是：

#include "std_msgs/String.h"

这个是引用了std_msgs/String消息，其在std_msgs包中。这是一个从std_msgs包中的 String.msg 文件中自动生成的头文件。

初始化ROS节点。

ros::init(argc, argv, "talker");

这允许ROS通过命令行进行重命名（现在并不重要）。这里我们只需要知道指定节点的名字为“talker”。
注意在一个运行的系统中，节点的名字必须唯一。同时，这里使用的名字为 base name，也就是在命名时不能有 (/) 或者 (~)等字符。

创建一个此处理节点的“句柄”。

ros::NodeHandle n;

创建的第一个节点“句柄”将实际执行节点的初始化，而最后一个NodeHandle销毁将清理所有节点使用的资源。

ros::Publisher chatter_pub = n.advertise("chatter", 1000);

告诉master我们将要通过话题“chatter”发布一个类型为std_msgs::String的消息。这会让master告诉所有正在接听"chatter"的节点说我们将会在此topic上发布消息。第二个参数是我们发布队列的大小。在此情况下，如果我们发布的过快，其在开始丢弃“旧数据”之前为我们最大存储1000个数据。
NodeHandle::advertise() 返回了一个ros::Publisher对象。其满足两个目标：

• 1）其包含一个publish()方法，能够让你发布数据到你创建时所指定的topic。
• 2）当其超出范围时，其将自动停止广播。

ros::Rate对象允许我们来指定循环的频率。其将跟踪自上次调用 Rate::sleep()之后的时间，并休眠对应数量的时间。

ros::Rate loop_rate(10);

这里我们想要以10Hz的频率进行运行。

  int count = 0;
  while (ros::ok())
  {

在默认情况下，roscpp将安装一个SIGINT处理程序，该处理程序提供Ctrl + C处理，如果检测到输入了Ctrl + C，会导致ros:：ok（）返回false。

在下面几种情况中，ros:：ok（）将返回false：

• SIGINT处理器接收到了Ctrl + C
• 我们被另外一个同名节点踢出了网络
• 另外一个程序调用了ros::shutdown()
• 所有的os::NodeHandles 都被销毁了

一旦ros:：ok（）返回false，所有的ROS调用都将失效。

   std_msgs::String msg;

   std::stringstream ss;
   ss << "hello world " << count;
   msg.data = ss.str();

我们将使用一个消息自适应类发布消息，通常从一个msg文件中生成。也可以有更加复杂的数据类型，但是现在，我们将使用只有一个成员“data”的标准String 消息。

上述代码片段的含义为：定义一个标准std_msgs中的String类型变量“msg”。然后定义一个字符串流ss。不断输入"hello
world "和count变量的值。再赋值给变量“msg”的成员data。

现在，我们真正的广播这个message给接收它的节点。

chatter_pub.publish(msg);

使用publish()方法。

ROS_INFO是我们常用的替代printf/cout的函数。

ROS_INFO("%s", msg.data.c_str());

在这个简单的程序中，调用ros::spinonce()不是必须的，因为我们没有接收任何回电。然而，如果我们想要在此程序中添加一个订阅者，并且没有在这里添加ros::spinonce()，我们就永远不会收到调用，因此不管怎么样添加这句话是一个较好的方法。

ros::spinOnce()

最后，我们使用ros::Rate 所定义的 loop_rate对象休眠一定的时间，来保证我们的10Hz发布频率。

loop_rate.sleep();

下面，我们对上述过程进行一个简要总结：

• 初始化ROS系统
• 广播我们将在“chatter”话题发布的 std_msgs/String消息到master
• 以每秒10Hz的频率进行循环

完成了上述工作之后，我们需要写一个节点来接收消息。

写一个Subscriber节点 Subscriber节点源码

在src文件夹目录下创建一个listener.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, "listener");

  
  ros::NodeHandle n;

  
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  
  ros::spin();

  return 0;
}

Subscriber节点源码解释

现在，我们再来了解一下Subscriber节点的源码片段，其中在前述Publisher中已经解释过的就不再叙述了。

首先是一个回调函数，当有新消息到达chatter topic时就会调用这个函数。消息是通过一个boost shared_ptr指针传递的，这意味着我们可以存储他们，而不需要担心它们被删除了，并且不需要复制底层数据。

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

通过下述命令订阅chatter话题，当有新消息到达时，调用上述的chatterCallback函数。第二个参数是队列的大小，以防我们不能足够快的处理消息。在此情况下，如果队列达到了1000个消息，我们将在新消息到达时丢弃旧数据。

ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

NodeHandle::subscribe()返回一个ros::Subscriber对象，我们必须使用到想要取消订阅为止。当Subscriber对象被销毁时，其将自动的取消对chatter话题的订阅。
一些版本的subscribe()函数允许我们调用一个类的成员函数，或者甚至可以指定Boost.function对象可调用的任何函数。

循环的最后，加入一句ros::spin();，尽快的调用消息回调。如果没有用到它也不必担心，因为其不会占用太多CPU资源。当ros::ok() 返回false时，ros::spin()将会退出，这意味着ros::shutdown()已经被 Ctrl+C 或者 主程序告诉我们结束 或者 手动 *** 作 所调用。

ros::spin();

最后，我们还是总结一下整个程序的流程：

• 初始化ROS系统
• 订阅“chatter”话题
• ros::spin()，等待消息到达
• 当消息到达后，调用回调函数chatterCallback()

编译节点

在之前我们使用catkin_create_pkg命令创建beginner_tutorials包的过程中自动生成了一个 package.xml文件 和 一个CMakeLists.txt文件。
生成的CMakeLists.txt文件形式如下：

cmake_minimum_required(VERSION 3.0.2)
project(beginner_tutorials)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## 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
  rospy
  std_msgs
  message_generation
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPonENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPonENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
add_message_files(
   FILES
   Num.msg
 )

## Generate services in the 'srv' folder
add_service_files(
   FILES
   AddTwoInts.srv
 )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
   DEPENDENCIES
   std_msgs
 )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPonENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES beginner_tutorials
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/beginner_tutorials.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/beginner_tutorials_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

在CMakeLists.txt文件的底部添加下述内容：

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

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

如果我们对CMake略有了解，则会意识到这将会创建两个可执行文件 talker 和 listener，将默认放入devel文件夹中，路径为：

 ~/catkin_ws/devel/lib/

注意，我们还必须添加可执行文件的目标依赖项到消息生成目标。

add_dependencies(talker beginner_tutorials_generate_messages_cpp)

这确保在使用此包之前生成该包的消息头。如果您在catkin工作区内使用来自其他包的消息，我们还需要将依赖项添加到它们各自的生成目标，因为catkin并行构建所有项目。从ROS的Groovy版本开始，我们可以使用以下变量来依赖于所有必要的目标：

target_link_libraries(talker ${catkin_LIBRARIES})

接下来使用catkin_make来编译生成我们想要的可执行文件。

# In your catkin workspace
$ cd ~/catkin_ws
$ catkin_make  

测试Publisher和Subscriber节点

编译之后，我们启动roscore，然后打开两个terminal窗口，分别使用rosrun启动talker和listener两个节点，执行命令如下：

$ roscore  

启动talker

$ rosrun beginner_tutorials talker

启动listener

rosrun beginner_tutorials listener

输出结果：

当然，我们也可以在~/catkin_ws/devel/lib/beginner_tutorials文件夹下找到生成的可执行文件，直接调用两个文件，进行运行节点。

./talker
./listener