Note: This tutorial assumes that you have completed the previous tutorials: understanding ROS services and parameters.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Viết một Publisher và Subscriber (C++) đơn giản

Description: Hướng dẫn để viết một nút publisher và subscriber trong C++.

Tutorial Level: BEGINNER

Next Tutorial: Examining the simple publisher and subscriber

Tạo một nút Publisher

"Nút" là thuật ngữ ROS cho một tệp thực thi được kết nối với mạng ROS. Ở đây, chúng ta sẽ tạo ra một nút xuất thông tin ("talker"), nó sẽ liên tục phát một tin nhắn.

Thay đổi thư mục vào trong gói beginner_tutorials, bạn đã tạo ra ở hướng dẫn trước creating a rosbuild package :

roscd beginner_tutorials

Thay đổi thư mục đến gói beginner_tutorials bạn đã tạo trong hướng dẫn catkin workspace:

roscd beginner_tutorials

Tạo một thư mục 'src' trong beginner_tutorials:

mkdir src

Thư mục này sẽ chứa tất cả tập tin cho gói beginner_tutorials.

Tạo tập tin src/talker.cpp trong gói beginner_tutorials package và chép vào như sau:

https://raw.github.com/ros/ros_tutorials/kinetic-devel/roscpp_tutorials/talker/talker.cpp

  27 #include "ros/ros.h"
  28 #include "std_msgs/String.h"
  29 
  30 #include <sstream>
  31 
  32 /**
  33  * This tutorial demonstrates simple sending of messages over the ROS system.
  34  */
  35 int main(int argc, char **argv)
  36 {
  37   /**
  38    * The ros::init() function needs to see argc and argv so that it can perform
  39    * any ROS arguments and name remapping that were provided at the command line.
  40    * For programmatic remappings you can use a different version of init() which takes
  41    * remappings directly, but for most command-line programs, passing argc and argv is
  42    * the easiest way to do it.  The third argument to init() is the name of the node.
  43    *
  44    * You must call one of the versions of ros::init() before using any other
  45    * part of the ROS system.
  46    */
  47   ros::init(argc, argv, "talker");
  48 
  49   /**
  50    * NodeHandle is the main access point to communications with the ROS system.
  51    * The first NodeHandle constructed will fully initialize this node, and the last
  52    * NodeHandle destructed will close down the node.
  53    */
  54   ros::NodeHandle n;
  55 
  56   /**
  57    * The advertise() function is how you tell ROS that you want to
  58    * publish on a given topic name. This invokes a call to the ROS
  59    * master node, which keeps a registry of who is publishing and who
  60    * is subscribing. After this advertise() call is made, the master
  61    * node will notify anyone who is trying to subscribe to this topic name,
  62    * and they will in turn negotiate a peer-to-peer connection with this
  63    * node.  advertise() returns a Publisher object which allows you to
  64    * publish messages on that topic through a call to publish().  Once
  65    * all copies of the returned Publisher object are destroyed, the topic
  66    * will be automatically unadvertised.
  67    *
  68    * The second parameter to advertise() is the size of the message queue
  69    * used for publishing messages.  If messages are published more quickly
  70    * than we can send them, the number here specifies how many messages to
  71    * buffer up before throwing some away.
  72    */
  73   ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  74 
  75   ros::Rate loop_rate(10);
  76 
  77   /**
  78    * A count of how many messages we have sent. This is used to create
  79    * a unique string for each message.
  80    */
  81   int count = 0;
  82   while (ros::ok())
  83   {
  84     /**
  85      * This is a message object. You stuff it with data, and then publish it.
  86      */
  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();
  92 
  93     ROS_INFO("%s", msg.data.c_str());
  94 
  95     /**
  96      * The publish() function is how you send messages. The parameter
  97      * is the message object. The type of this object must agree with the type
  98      * given as a template parameter to the advertise<>() call, as was done
  99      * in the constructor above.
 100      */
 101     chatter_pub.publish(msg);
 102 
 103     ros::spinOnce();
 104 
 105     loop_rate.sleep();
 106     ++count;
 107   }
 108 
 109 
 110   return 0;
 111 }

Giải thích mã

Bây giờ hãy xem xét từng đoạn mã

  27 #include "ros/ros.h"
  28 

ros/ros.h là một tiện ích bao gồm tất cả các headers cần thiết để sử dụng các phần công khai (common public pieces) phổ biến nhất của hệ thống ROS.

  28 #include "std_msgs/String.h"
  29 

This includes the std_msgs/String message, nằm trong gói std_msgs . Đây là header được tự động tạo ra từ tập tin String.msg trong gói (package). Để có thêm thông tin về định nghĩa message, xem trang msg.

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

Khởi tạo ROS. Cho phép ROS đặt lại tên thông qua công cụ dòng lệnh -- tạm thời không quan tâm. Đây cũng là nơi đặt tên cho một nút (ở đây tên là 'talker'). Tên nút phải là riêng biệt khi chạy trong hệ thống.

Tên dùng phải là một base name, vd. không có a / trong đó.

  54   ros::NodeHandle n;

Tạo một handle cho node xử lý này. NodeHandle đầu tiên tạo ra sẽ làm việc là khởi tạo nút, và destructed sẽ hủy bỏ các nguồn tài nguyên mà nút đã sử dụng.

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

Khai báo cho Ros master về loại của tin nhắn được xuật bản std_msgs/String trên topic chatter. Điều này cho phép master thông báo với bất kỳ node nào đang lắng nghe khi chatter xuất dữ liệu trên topic đó. Đối số thứ hai là kích thước của hàng đợi publishing. Trong trường hợp này nếu chúng ta xuất ra quá nhanh nó sẽ giữ tối đa 1000 tin nhắn(message) trước khi bắt đầu xóa những mesage cũ.

NodeHandle::advertise() trả về một đối tượng ros::Publisher, nhằm hai mục đích: 1) nó chứa một method publish() để bạn xuất tin nhắn trên topic đã tạo, và 2) khi không còn chạy nó sẽ tự động unadvertise.

  75   ros::Rate loop_rate(10);

đối tượng ros::Rate cho phép bạn thiết lập tầng số trong vòng lặp. Nó sẽ theo dõi khoảng thời gian kể từ lần gọi cuối cùng Rate::sleep(), và sleep chính xác khoảng thời gian.

Trong trường hợp này chúng ta sẽ chạy với 10Hz.

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

Mặc định roscpp sẽ cài một SIGINT handler để cung cấp cơ chế Ctrl-C handling sẽ làm ros::ok() trả về false nếu trường hợp này xảy ra.

ros::ok()sẽ trả về false nếu:

  • Một SIGINT được nhận (Ctrl-C)
  • Bị xung đột và văng khỏi mạng (kicked off the network) bởi nút khác có cùng tên
  • ros::shutdown() được gọi bởi một aplication.

  • Tất cả ros::NodeHandles bị đóng (destroyed)

Khi ros::ok() trả về false, tất cả ROS calls sẽ fail.

  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();

Chúng ta xuất tin nhắn trên ROS dùng một lớp message-adapted, tự động tạo ra từ msg file. Có thể tạo ra những kiểu dữ liệu phức tạp, tạm thời trong ví dụ này dùng tin nhắn chuẩn String message, chỉ có một trường dữ liệu (one member: "data").

 101     chatter_pub.publish(msg);

Bây giờ chúng ta thực sự xuất message đến node đã có kết nối với topic xuất.

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

ROS_INFO và friends được dùng để thay thế cho printf/cout. Xem rosconsole documentation để có thêm thông tin.

 103     ros::spinOnce();

gọi ros::spinOnce() ở đây không cần thiết cho chương trình đơn giản, bởi vì chúng ta không nhận callbacks. Hơn thế, nếu bạn thêm một subscription vào application, và không có ros::spinOnce(), hàm callbacks sẽ không được gọi. Thế nên tốt nhất nên thêm vào khi có thể.

 105     loop_rate.sleep();

Bây giờ chúng ta dùng đối tượng ros::Rate để sleep trong khoảng thời gian để đạt được tầng số xuất 10Hz.

Here's the condensed version of what's going on:

  • Khởi tạo hệ thống ROS
  • Advertise that we are going to be publishing std_msgs/String messages on the chatter topic to the master

  • Lặp trong khi xuất messages đến chatter 10 lần trong một giây

Bây giờ chúng ta cần viết node để nhận messsages.

Viết node Subscriber

The Code

Tạo tập tin src/listener.cpp trong gói beginner_tutorials và sao chép như sau:

https://raw.github.com/ros/ros_tutorials/kinetic-devel/roscpp_tutorials/listener/listener.cpp

  28 #include "ros/ros.h"
  29 #include "std_msgs/String.h"
  30 
  31 /**
  32  * This tutorial demonstrates simple receipt of messages over the ROS system.
  33  */
  34 void chatterCallback(const std_msgs::String::ConstPtr& msg)
  35 {
  36   ROS_INFO("I heard: [%s]", msg->data.c_str());
  37 }
  38 
  39 int main(int argc, char **argv)
  40 {
  41   /**
  42    * The ros::init() function needs to see argc and argv so that it can perform
  43    * any ROS arguments and name remapping that were provided at the command line.
  44    * For programmatic remappings you can use a different version of init() which takes
  45    * remappings directly, but for most command-line programs, passing argc and argv is
  46    * the easiest way to do it.  The third argument to init() is the name of the node.
  47    *
  48    * You must call one of the versions of ros::init() before using any other
  49    * part of the ROS system.
  50    */
  51   ros::init(argc, argv, "listener");
  52 
  53   /**
  54    * NodeHandle is the main access point to communications with the ROS system.
  55    * The first NodeHandle constructed will fully initialize this node, and the last
  56    * NodeHandle destructed will close down the node.
  57    */
  58   ros::NodeHandle n;
  59 
  60   /**
  61    * The subscribe() call is how you tell ROS that you want to receive messages
  62    * on a given topic.  This invokes a call to the ROS
  63    * master node, which keeps a registry of who is publishing and who
  64    * is subscribing.  Messages are passed to a callback function, here
  65    * called chatterCallback.  subscribe() returns a Subscriber object that you
  66    * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
  67    * object go out of scope, this callback will automatically be unsubscribed from
  68    * this topic.
  69    *
  70    * The second parameter to the subscribe() function is the size of the message
  71    * queue.  If messages are arriving faster than they are being processed, this
  72    * is the number of messages that will be buffered up before beginning to throw
  73    * away the oldest ones.
  74    */
  75   ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  76 
  77   /**
  78    * ros::spin() will enter a loop, pumping callbacks.  With this version, all
  79    * callbacks will be called from within this thread (the main one).  ros::spin()
  80    * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
  81    */
  82   ros::spin();
  83 
  84   return 0;
  85 }

Diễn giải Code

Bây giờ, hãy phân tích từng đoạn, bỏ qua những phần đã giải thích ở trên.

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

Đây là hàm callback sẽ được gọi khi có message mới được nhận trên topic chatter. Tin nhắn được đưa đến boost shared_ptr, trong đó nó sẽ lưu lại data.

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

Đăng ký vào topic chatter với master. ROS sẽ gọi hàm chatterCallback() khi có một messge mới đến. Thông số thứ 2 là kích thước hàng chờ (the queue size), trong trường hợp chúng ta xử lý message không kịp. Trong trường hợp, nếu hàng đợi vượt quá 1000 messages, nó sẽ xóa bỏ message cũ khi có message mới đến.

NodeHandle::subscribe() trả về một đối tượng ros::Subscriber , bạn sẽ giữ nó cho đến khi muốn unsubscribe. Khi đối tượng Subscriber bị hủy, nó sẽ tự unsubscribe từ topic chatter.

Phiên bản của hàm NodeHandle::subscribe() cho phép bạn chỉ ra một lớp (class member function) , hoặc bất cứ hàm có thể gọi bởi đối tượng Boost.Function. Xem roscpp overview để có thêm thông tin.

  82   ros::spin();

ros::spin() điểm vào vòng lặp, gọi message callbacks nhanh nhất có thể. (Don't worry though, if there's nothing for it to do it won't use much CPU. ros::spin() will exit once ros::ok() returns false, which means ros::shutdown() has been called, either by the default Ctrl-C handler, the master telling us to shutdown, or it being called manually.)

Có những cách khác để gọi callbacks, nhưng chúng ta sẽ không bàn ở đây. Gói roscpp_tutorials có sẵn minh họa aplications. Xem roscpp overview để biết thêm thông tin.

Một lần nữa, tóm tắt lại những gì cần làm:

  • Khởi tạo hệ thống ROS
  • Đăng ký (subscribe) topic của chatter

  • Spin, chờ cho tin nhắn gưởi đến
  • Khi tin nhắn đến, gọi hàm chatterCallback()

Tạo một nodes

roscreate-pkg sẽ tạo một Makefile và CMakeLists.txt mặc định cho package.

$ rosed beginner_tutorials CMakeLists.txt

Nó sẽ trông giống như sau:

  • cmake_minimum_required(VERSION 2.4.6)
    include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
    
    # Set the build type.  Options are:
    #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
    #  Debug          : w/ debug symbols, w/o optimization
    #  Release        : w/o debug symbols, w/ optimization
    #  RelWithDebInfo : w/ debug symbols, w/ optimization
    #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
    #set(ROS_BUILD_TYPE RelWithDebInfo)
    
    rosbuild_init()
    
    #set the default path for built executables to the "bin" directory
    set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
    #set the default path for built libraries to the "lib" directory
    set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
    
    #uncomment if you have defined messages
    #rosbuild_genmsg()
    #uncomment if you have defined services
    #rosbuild_gensrv()
    
    #common commands for building c++ executables and libraries
    #rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
    #target_link_libraries(${PROJECT_NAME} another_library)
    #rosbuild_add_boost_directories()
    #rosbuild_link_boost(${PROJECT_NAME} thread)
    #rosbuild_add_executable(example examples/example.cpp)
    #target_link_libraries(example ${PROJECT_NAME})

Thêm vào những dòng sau ở cuối:

rosbuild_add_executable(talker src/talker.cpp)
rosbuild_add_executable(listener src/listener.cpp)

Điều này sẽ tạo hai executables, talkerlistener, trong đó mặc định sẽ nằm trong thư mục "bin".

Để có thêm thông tin về dùng Cmake với ROS, xem CMakeLists Bây giờ chạy make:

$ make

Tạo một node

Bạn có thể dùng catkin_create_pkg trong hướng dẫn trước để tạo một package.xml và một tập tin [catkin/CMakeLists.txt|CMakeLists.txt]].

Tập tin được tạo ra CMakeLists.txt sẽ giống như sau (với chỉnh sửa từ hướng dẫn Creating Msgs and Srvs và bỏ những comments và ví dụ không dùng):

https://raw.github.com/ros/catkin_tutorials/master/create_package_modified/catkin_ws/src/beginner_tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(DIRECTORY msg FILES Num.msg)
   9 add_service_files(DIRECTORY srv FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()

Đừng lo lắng về việc sửa ví dụ (#), đơn giản thêm những dòng sau vào cuối tập CMakeLists.txt:

include_directories(include ${catkin_INCLUDE_DIRS})

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)

Kết quả tập tin CMakeLists.txt sẽ giống như sau:

https://raw.github.com/ros/catkin_tutorials/master/create_package_pubsub/catkin_ws/src/beginner_tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(FILES Num.msg)
   9 add_service_files(FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()
  16 
  17 ## Build talker and listener
  18 include_directories(include ${catkin_INCLUDE_DIRS})
  19 
  20 add_executable(talker src/talker.cpp)
  21 target_link_libraries(talker ${catkin_LIBRARIES})
  22 add_dependencies(talker beginner_tutorials_generate_messages_cpp)
  23 
  24 add_executable(listener src/listener.cpp)
  25 target_link_libraries(listener ${catkin_LIBRARIES})
  26 add_dependencies(listener beginner_tutorials_generate_messages_cpp)

Nó sẽ tạo hai tập executables, talkerlistener, trong đó mặc định sẽ tạo ra trong thư mục devel space, vị trí mặc định bởi ~/catkin_ws/devel/lib/<package name>.

Chú ý rằng bạn cần thêm sự phụ thuộc cho executable targets:

add_dependencies(talker beginner_tutorials_generate_messages_cpp)

Điều này để bảo đảm rằng message headers của gói này được tạo ra trước khi sử dụng. Nếu bạn sử dụng messages từ gói khác packages trong thư mục catkin workspace, bạn cần thêm sự phụ thuộc , bởi vì catkin tạo tất cả project cùng lúc. Trong *Groovy* bạn có thể dùng tất cả các biến sau để đưa vào các target phụ thuộc:

target_link_libraries(talker ${catkin_LIBRARIES})

Bạn có thể gọi file thực thi trực tiếp hoặc bạn có thể dùng rosrun để gọi. Nó sẽ không có trong '<prefix>/bin' because that would pollute the PATH when installing your package to the system. Nếu bạn muốn file thực thi trong PATH tại thời điểm cài đặt, bạn có thể thiết đặt và cài đặt thư mục cài, xem: catkin/CMakeLists.txt

Để có thêm thông tin chi tiết về tập tin CMakeLists.txt xem: catkin/CMakeLists.txt

Bây giờ chạy catkin_make:

# In your catkin workspace
$ catkin_make

Chú ý: Hoặc nếu bạn thêm package mới, bạn cần báo cho catkin để chạy making bởi thêm lựa chọn --force-cmake. Xem thêm catkin/Tutorials/using_a_workspace#With_catkin_make.

Bây giờ bạn đã viết một publisher và subscriber đơn giản, hãy examine the simple publisher and subscriber.

Additional Resources

Dưới đây là một số tham khảo bổ sung đóng góp của cộng đồng:

Video Hướng dẫn

Video sau đây giới thiệu một hướng dẫn nhỏ giải thích cách viết và thử nghiệm một publisher và subscriber trong ROS bằng C + + và Python dựa trên ví dụ về talker/listener ở trên

Wiki: vn/ROS/Tutorials/WritingPublisherSubscriber(c++) (last edited 2017-08-10 06:40:20 by HoangGiang88)