ROS与C++入门教程-编写简单发布节点和订阅节点
ROS与C++入门教程-写简单发布节点和订阅节点
说明:
- 介绍用C++编写简单发布节点和订阅节点
准备
- 第三节已经建立的基本的工作空间catkin_ws和教程包beginner_tutorials
编写发布节点
- 进入教程目录,并新建源目录和发布节点talker.cpp(代码看参考github),实现发布hello world
$ roscd beginner_tutorials
$ mkdir src
$ touch src/talker.cpp
$ vim src/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;
}
代码分析:
- 代码:
#include "ros/ros.h"
解释:
- 导入ROS系统包含核心公共头文件
代码:
#include "std_msgs/String.h"
解释:
- 导入std_msgs/String消息头文件,这个是由std_msgs包的string.msg文件自动生成。
- std_msgs这是标准的消息包,可参考std_msgs ,更多的标准消息定义,可参考msg
代码:
ros::init(argc, argv, "talker");
解释:
- 初始化ROS,指定节点名称为“talker”,节点名称要保持唯一性。名称定义参考
代码:
ros::NodeHandle n;
解释:
- 实例化节点
代码:
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
解释:
- 发布一个消息类型为std_msgs/String,命名为chatter的话题。
- 定义消息队列大小为1000,即超过1000条消息之后,旧的消息就会丢弃。
代码:
ros::Rate loop_rate(10);
解释:
- 指定发布消息的频率,这里指10Hz,也即每秒10次
- 通过 Rate::sleep()来处理睡眠的时间来控制对应的发布频率。
代码:
int count = 0;
while (ros::ok())
{
解释:
- 默认roscpp会植入一个SIGINT处理机制,当按下Ctrl-C,就会让ros::ok()返回false,那循环就会结束。
- ros::ok() 返回false的几种情况:
- SIGINT收到(Ctrl-C)信号
- 另一个同名节点启动,会先中止之前的同名节点
- ros::shutdown()被调用
- 所有的ros::NodeHandles被销毁
代码:
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
解释:
- 实例化消息msg, 定义字符串流“hello world”并赋值给ss, 最后转成为字符串赋值给msg.data
代码:
chatter_pub.publish(msg);
解释:
- 实际发布的函数
代码:
ROS_INFO("%s", msg.data.c_str());
解释:
- 输出调试信息,更多调试相关参考Verbosity Levels, rosconsole
代码:
ros::spinOnce();
- 解释:
- 不是必需的,但是保持增加这个调用,是好习惯。
- 如果程序里也有订阅话题,就必需,否则回调函数不能起作用。
- 代码:
loop_rate.sleep();
- 解释:
- 根据之前ros::Rate loop_rate(10)的定义来控制发布话题的频率。定义10即为每秒10次
编写订阅节点
- 进入beginner_tutorials,新建src/listener.cpp文件,源码参考
cd beginner_tutorials
touch src/listener.cpp
vim 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;
}
代码分析:
- 代码:
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
- 解释:
- 定义回调函数chatterCallback,当收到chatter话题的消息就会调用这个函数。
- 消息通过boost shared_ptr(共享指针)来传递。
- 但收到消息,通过ROS_INFO函数显示到终端
- 代码:
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
解释:
- 定义订阅节点,名称为chatter,指定回调函数chatterCallback
- 但收到消息,则调用函数chatterCallback来处理。
- 参数1000,定义队列大小,如果处理不够快,超过1000,则丢弃旧的消息。
代码:
ros::spin();
- 解释:
- 调用此函数才真正开始进入循环处理,直到 ros::ok()返回false才停止。
构建节点
- 在之前使用catkin_create_pkg创建包beginner_tutorials,会得到 package.xml 和 CMakeLists.txt两个文件
- 增加如下代码到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)
- 完整代码类似:
cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)
## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
## Declare ROS messages and services
add_message_files(FILES Num.msg)
add_service_files(FILES AddTwoInts.srv)
## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)
## Declare a catkin package
catkin_package()
## Build talker and listener
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)
- 创建两个执行文件, talker and listener,默认在
~/catkin_ws/devel/lib/<package name>
- 注意,您必须为消息生成目标添加可执行目标的依赖项:
add_dependencies(talker beginner_tutorials_generate_messages_cpp)
- 这确保此包的消息头在使用前生成。如果你使用的信息在你的catkin工作空间的其他包里面,你需要添加依赖各自生成目标,因为catkin会平行构建的所有项目。
target_link_libraries(talker ${catkin_LIBRARIES})
- 进入工作空间编译
$ cd ~/catkin_ws
$ catkin_make
运行节点
- 新开终端执行:
$ roscore
- 新开终端执行talker:
source ~/catkin_ws/devel/setup.bash
rosrun beginner_tutorials talker
- 新开终端执行listener:
source ~/catkin_ws/devel/setup.bash
rosrun beginner_tutorials listener
- talker终端效果:
[ INFO] I published [Hello there! This is message [0]]
[ INFO] I published [Hello there! This is message [1]]
[ INFO] I published [Hello there! This is message [2]]
- listener终端效果:
[ INFO] Received [Hello there! This is message [20]]
[ INFO] Received [Hello there! This is message [21]]
[ INFO] Received [Hello there! This is message [22]]
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号