ROS发布者和订阅者，服务端和客户端
编写发布者和订阅者
编写节点
roscd xxx
C++
先生成一个src目录来存放源代码文件
mkdir src
cd src
发布者
在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");//初始化ROS。这使得ROS可以通过命令行进行名称重映射——不过目前不重要。这也是我们给节点指定名称的地方。节点名在运行的系统中必须是唯一的。注意：名称必须是基本名称，例如不能包含任何斜杠/。
/**
* 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;//为这个进程的节点创建句柄。创建的第一个NodeHandle实际上将执行节点的初始化，而最后一个被销毁的NodeHandle将清除节点所使用的任何资源。
/**
* 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);//告诉主节点我们将要在chatter话题上发布一个类型为std_msgs/String的消息。这会让主节点告诉任何正在监听chatter的节点，我们将在这一话题上发布数据。第二个参数是发布队列的大小。在本例中，如果我们发布得太快，它将最多缓存1000条消息，不然就会丢弃旧消息。
//NodeHandle::advertise()返回一个ros::Publisher对象，它有2个目的：其一，它包含一个publish()方法，可以将消息发布到创建它的话题上；其二，当超出范围时，它将自动取消这一宣告操作。
ros::Rate loop_rate(10);//ros::Rate对象能让你指定循环的频率。它会记录从上次调用Rate::sleep()到现在已经有多长时间，并休眠正确的时间。在本例中，我们告诉它希望以10Hz运行。
/**
* 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())
{
//默认情况下，roscpp将安装一个SIGINT处理程序，它能够处理Ctrl+C操作，让ros::ok()返回false。
//ros::ok()在以下情况会返回false：
//收到SIGINT信号（Ctrl+C）
//被另一个同名的节点踢出了网络
//ros::shutdown()被程序的另一部分调用
//所有的ros::NodeHandles都已被销毁
//一旦ros::ok()返回false, 所有的ROS调用都会失败。
/**
* 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上广播消息，该类通常由msg文件生成。更复杂的数据类型也可以，不过我们现在将使用标准的String消息，它有一个成员：data。
ROS_INFO("%s", msg.data.c_str());//ROS_INFO和它的朋友们可用来取代printf/cout
/**
* 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();//此处调用ros::spinOnce()对于这个简单程序来说没啥必要，因为我们没有接收任何回调。然而，如果要在这个程序中添加订阅，但此处没有ros::spinOnce()的话，回调函数将永远不会被调用。所以还是加上吧。
loop_rate.sleep();//现在我们使用ros::Rate在剩下的时间内睡眠，以让我们达到10Hz的发布速率。
++count;
}
return 0;
}
总结为以下几点
- 初始化ROS系统
- 向主节点宣告我们将要在chatter话题上发布std_msgs/String类型的消息
- 以每秒10次的速率向chatter循环发布消息
订阅者
在src目录下创建talker.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());
}//这是一个回调函数，当有新消息到达chatter话题时它就会被调用。该消息是用boost shared_ptr智能指针传递的，这意味着你可以根据需要存储它，即不用担心它在下面被删除，又不必复制底层（underlying）数据。
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);//通过主节点订阅chatter话题。每当有新消息到达时，ROS将调用chatterCallback()函数。第二个参数是队列大小，以防我们处理消息的速度不够快。在本例中，如果队列达到1000条，再有新消息到达时旧消息会被丢弃。
//NodeHandle::subscribe()返回一个ros::Subscriber对象，你必须保持它，除非想取消订阅。当Subscriber对象被析构，它将自动从chatter话题取消订阅。
//还有另一些版本的NodeHandle::subscribe()函数，可以让你指定为类的成员函数，甚至是可以被Boost.Function对象调用的任何函数。
/**
* 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();//ros::spin()启动了一个自循环，它会尽可能快地调用消息回调函数。不过不要担心，如果没有什么事情，它就不会占用太多CPU。另外，一旦ros::ok()返回false，ros::spin()就会退出，这意味着ros::shutdown()被调用了，主节点让我们关闭（或是因为按下Ctrl+C，它被手动调用）。
return 0;
}
构建节点
将这几行添加到CMakeLists.txt文件的底部：
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker xxx_generate_messages_cpp)
#这确保了在使用此包之前生成了该包的消息头。如果使用来自你catkin工作空间中的其他包中的消息，则还需要将依赖项添加到各自的生成目标中，因为catkin将所有项目并行构建。
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener xxx_generate_messages_cpp)
这将创建两个可执行文件talker和listener，默认情况下，它们将被放到软件包目录下的devel空间中，即~/workspace/devel/lib/
运行catkin_make
cd ~/workspace
catkin_make
Python
发布者
首先创建一个scripts目录来存放的Python脚本：
mkdir scripts
cd scripts
在scripts中创建一个talker.py，将以下代码复制进去
#!/usr/bin/env python
# 每个Python ROS节点的最开头都有这个声明。第一行确保脚本作为Python脚本执行。
# license removed for brevity
import rospy
from std_msgs.msg import String
# 如果要编写ROS节点，则需要导入rospy。std_msgs.msg的导入则是为了使我们能够重用std_msgs/String消息类型（即一个简单的字符串容器）来发布。
def talker():
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
#这部分代码定义了talker与其他ROS部分的接口。pub = rospy.Publisher("chatter", String, queue_size=10)声明该节点正在使用String消息类型发布到chatter话题。这里的String实际上是std_msgs.msg.String类。
#queue_size参数是用于在订阅者接收消息的速度不够快的情况下，限制排队的消息数量。
#下一行的rospy.init_node(NAME, ...)非常重要，因为它把该节点的名称告诉了rospy——只有rospy掌握了这一信息后，才会开始与ROS主节点进行通信。在本例中，你的节点将使用talker名称。注意：名称必须是基本名称，例如不能包含任何斜杠/。
#anonymous = True会让名称末尾添加随机数，来确保节点具有唯一的名称。
rate = rospy.Rate(10) # 10hz
#此行创建一个Rate对象rate。借助其方法sleep()，它提供了一种方便的方法，来以你想要的速率循环。它的参数是10，即表示希望它每秒循环10次（只要我们的处理时间不超过十分之一秒）！
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
rate. Sleep()
#这个循环是一个相当标准的rospy结构：检查rospy.is_shutdown()标志，然后执行代码逻辑。你必须查看is_shutdown()以检查程序是否应该退出（例如有Ctrl+C或其他）。在本例中，代码逻辑即对public .publish(hello_str)的调用，它将一个字符串发布到chatter话题。循环的部分还调用了rate.sleep()，它在循环中可以用刚刚好的睡眠时间维持期望的速率。
#你可能也见过rospy.sleep()，它和time.sleep()类似，不同的是前者还能用于模拟时间（参见Clock）。
#此循环还调用了rospy.loginfo(str)，它有3个任务：打印消息到屏幕上；把消息写入节点的日志文件中；写入rosout。rosout是一个方便的调试工具：您可以使用rqt_console来拉取消息，而不必在控制台窗口找你节点的输出。
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:#除了标准的Python __main__检查，还会捕获一个rospy.ROSInterruptException异常，当按下Ctrl+C或节点因其他原因关闭时，这一异常就会被rospy.sleep()和rospy.Rate.sleep()抛出。
#引发此异常的原因是不会意外地在sleep()之后继续执行代码。
pass
给权限
chmod +x talker.py
然后将以下内容添加到CMakeLists.txt文件。这样可以确保正确安装Python脚本，并使用合适的Python解释器。
catkin_install_python(PROGRAMS scripts/talker.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
订阅者
在scripts中创建一个listener.py，将以下代码复制进去
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("chatter", String, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
#这声明你的节点订阅了chatter话题，类型是std_msgs.msgs.String。当接收到新消息时，callback函数被调用，消息作为第一个参数。
#我们还稍微更改了对rospy.init_node()的调用。我们添加了anonymous=True关键字参数。ROS要求每个节点都有一个唯一的名称，如果出现具有相同名称的节点，则会与前一个节点发生冲突，这样一来，出现故障的节点很容易地被踢出网络。anonymous=True标志会告诉rospy为节点生成唯一的名称，这样就很容易可以有多个listener.py一起运行。
#最后再补充一下，rospy.spin()只是不让你的节点退出，直到节点被明确关闭。与roscpp不同，rospy.spin()不影响订阅者回调函数，因为它们有自己的线程。
if __name__ == '__main__':
listener()
给权限
chmod +x listener.py
然后将以下内容添加到CMakeLists.txt文件。这样可以确保正确安装Python脚本，并使用合适的Python解释器。
catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
构建节点
cd ~/catkin_ws
catkin_make
检验发布者和订阅者
roscore
cd ~/workspace
source ./devel/setup. Bash
运行发布者
运行建立的talker发布者
rosrun xxx talker # (C++)
rosrun xxx talker.py # (Python)
运行订阅者
运行建立的listener订阅者
rosrun beginner_tutorials listener # (C++)
rosrun beginner_tutorials listener.py # (Python)
发布者和订阅者到此结束
构建服务端和客户端
C++
服务节点
在xxx包中创建src/add_two_ints_server.cpp文件并粘贴以下内容进去
#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
bool add(beginner_tutorials::AddTwoInts::Request &req,
beginner_tutorials::AddTwoInts::Response &res)
//这个函数提供了AddTwoInts服务，它接受srv文件中定义的请求（request）和响应（response）类型，并返回一个布尔值。
{
res.sum = req.a + req.b;
ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
ROS_INFO("sending back response: [%ld]", (long int)res.sum);
return true;
//此处，两个整数被相加，和已经存储在了响应中。然后记录一些有关请求和响应的信息到日志中。完成后，服务返回true。
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_two_ints_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("add_two_ints", add);//在这里，服务被创建，并在ROS中宣告。
ROS_INFO("Ready to add two ints.");
ros::spin();
return 0;
}
客户端节点
在xxx包中创建src/add_two_ints_client.cpp文件并粘贴以下内容进去：
#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_two_ints_client");
if (argc != 3)
{
ROS_INFO("usage: add_two_ints_client X Y");
return 1;
}
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");//这将为add_two_ints服务创建一个客户端。ros::ServiceClient对象的作用是在稍后调用服务。
beginner_tutorials::AddTwoInts srv;
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);//这里我们实例化一个自动生成的服务类，并为它的request成员赋值。一个服务类包括2个成员变量：request和response，以及2个类定义：Request和Response。
if (client. Call(srv))//此处实际上调用了服务。由于服务调用被阻塞，它将在调用完成后返回。如果服务调用成功，call()将返回true，并且srv.response中的值将是有效的。如果调用不成功，则call()将返回false且srv.response的值将不可用。
{
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}
return 0;
}
构建节点
再来编辑一下xxx里面的CMakeLists.txt文件，文件位于~/workspacepath/src/xxx/CMakeLists.txt，并将下面的代码添加在文件末尾：
add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server xxx_gencpp)
add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client xxx_gencpp)
这将创建两个可执行文件add_two_ints_server和add_two_ints_client，默认情况下，它们将被放到软件包目录下的devel空间中，即~/workspace/devel/lib/
现在可以运行catkin_make：
cd ~/workspace
catkin_make
Python
服务节点
在xxx包中创建scripts/add_two_ints_server.py文件并粘贴以下内容进去：
#!/usr/bin/env python
from __future__ import print_function
from beginner_tutorials.srv import AddTwoInts,AddTwoIntsResponse
import rospy
def handle_add_two_ints(req):
print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
return AddTwoIntsResponse(req.a + req.b)
def add_two_ints_server():
rospy.init_node('add_two_ints_server')
s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
#这声明了一个名为add_two_ints的新服务，其服务类型为AddTwoInts。所有的请求（request）都传递给了handle_add_two_ints函数。handle_add_two_ints被AddTwoIntsRequest的实例调用，返回AddTwoIntsResponse实例。
print("Ready to add two ints.")
rospy.spin()
if __name__ == "__main__":
add_two_ints_server()
别忘了给节点执行权限：
chmod +x scripts/add_two_ints_server.py
然后将以下内容添加到CMakeLists.txt文件。这样可以确保正确安装Python脚本，并使用合适的Python解释器。
catkin_install_python(PROGRAMS scripts/add_two_ints_server.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
客户端节点
在xxx包中创建scripts/add_two_ints_client.py文件并粘贴以下内容进去：
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
from beginner_tutorials.srv import *
def add_two_ints_client(x, y):
rospy.wait_for_service('add_two_ints')
#可以让在add_two_ints服务可用之前一直阻塞。
try:
add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
#为服务的调用创建了句柄
resp1 = add_two_ints(x, y)
return resp1.sum
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def usage():
return "%s [x y]"%sys.argv[0]
if __name__ == "__main__":
if len(sys.argv) == 3:
x = int(sys.argv[1])
y = int(sys.argv[2])
else:
print(usage())
sys.exit(1)
print("Requesting %s+%s"%(x, y))
print("%s + %s = %s"%(x, y, add_two_ints_client(x, y)))
给节点权限
chmod +x scripts/add_two_ints_client.py
然后，在你的CMakeLists.txt中编辑catkin_install_python()调用，就像这样：
catkin_install_python(PROGRAMS scripts/add_two_ints_server.py scripts/add_two_ints_client.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
构建节点
运行catkin_make:
cd ~/workspace
catkin_make
检验服务和客户端
运行服务端
rosrun xxx add_two_ints_server # (C++)
rosrun xxx add_two_ints_server.py # (Python)
运行客户端
rosrun beginner_tutorials add_two_ints_client 1 3 # (C++)
rosrun beginner_tutorials add_two_ints_client.py 1 3 # (Python)
至此，客户端和服务端结束
本文来自博客园，作者：不想要名字，转载请注明原文链接：https://www.cnblogs.com/xuwithbean/articles/17809789.html
