写一个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 <sstream>
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "talker");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
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<std_msgs::String>("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"
/**
* This tutorial demonstrates simple receipt of messages over the ROS system.
*/
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "listener");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The subscribe() call is how you tell ROS that you want to receive messages
* on a given topic. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called chatterCallback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). ros::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
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/<package name>
注意,我们还必须添加可执行文件的目标依赖项到消息生成目标。
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