ROS学习记录6——节点的编写逻辑与实现(C++/Python)
一.流程简述
我花了一个稍微简单但细节上又不太准确的图：
节点的功能就是参与数据运算或者对数据进行操作。比如我们可以由一个节点计算图片坐标，然后通过控制下位机的节点订阅坐标，实现获取坐标并与下位机通信，实现“识图-移动"。这只是个简单的例子。那么结合上图我们看看一个节点的核心要素（这里就先用C++展示了）：
#include "ros/ros.h"导入头文件，确保能基于ROS编程。int main(int argc, char **argv)主函数需要写成这样的标准化入口。ros::init(argc, argv, "节点名")初始化节点这个才是我们最真实的节点名，系统内显示的是这个。ros::NodeHandle n创建一个句柄，后续如果有对节点有其他操作，需通过句柄，所以不可或缺。对于句柄的理解可以百度，简单来说，你大概可以理解为一个为了方便外部调用其代码而生成的"地址"。ros::Rate loop_rate(int 频率)如果有循环体且，要控制循环运行的频率，需要调用，单位Hz。ROS_INFO()会被log记录的printf()ros::ok()用户判断该节点是否可用，出现以下情况会返回false：(反正能用就是true)- SIGINT 被触发 (Ctrl-C(关闭终端))
- 被另一同名节点踢出 ROS 网络
- ros::shutdown() 被程序的另一部分调用
- 节点中的所有 ros::NodeHandles 都已经被销毁
ros::spinOnce()如果有回调函数，需要添加此语句，否则回调函数不能被触发。loop_rate.sleep()通过调用 ros::Rate 对象来休眠一段时间控制运行频率
这大约就是一个节点所需要用到的东西。那么我们来构成一个最小代码看看：
#include "ros/ros.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "timer");
ros::NodeHandle n;
ros::Rate loop_rate(1);
while (ros::ok())
{
ROS_INFO("1s passed");
// ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
然后是常见的4个方法的参数说明：
-
基类
ros::Publisher，方法：advertise(const string &topic, uint32_t queue_size, bool latch=false)向 topic 发送数据- 第一个参数是 topic 的名称。
- 第二个参数是 数据队列的最大长度，如果缓存中 topic msg 超过这个长度，那么旧的消息就会被删除。
- 第三个参数表明是否在 topic 上启用锁存功能。保留最后一次话题的数据，如果订阅频率>更新频率，那么会发送最后一次更新的话题数据。
-
基类
ros::Subscriber，方法：subscribe(const string &topic, uint32_t queue_size, void(*)(M))接受 topic 中的数据- 第一个参数是 topic 的名称。
- 第二个参数是缓冲区 msg 队列的最大长度，如果接收的数据超出了缓存长度，则丢掉旧的数据。
- 第三个参数是回调函数指针，指向回调函数，
-
advertiseService(const string &service, bool(*srv_func)(Mreq &, Mres &))创建服务的server端- 第一个参数是该服务提供的 service 名称。
- 第二个参数是服务函数的指针，指向服务函数。服务函数接受两个参数，分别为请求和响应。
-
serviceClient(const string &service_name, bool persistent=false)创建服务的 client端- 第一个参数为 service 名称。
- 第二个参数用于确定是否为长链接。短连接省资源，但每次通信需多发握手包，时间稍微变长；长链接费资源，但有心跳包机制，发送接收数据不用握手。
好了，那我们来看看节点编程里常用的话题节点与服务节点吧:
二.通信节点编写
这个就拿前两篇的文章来说明吧：
1.话题篇
① C++版
发布节点
#include "ros/ros.h"
#include "top/Name.h"
// 引用消息的头文件，格式为 "功能包名/消息名.h"
#include "sstream"
// 基于C++编程，不解释
int main(int argc, char **argv)
{
std::stringstream ss;
// 基于C++编程，不解释
top::Name msg;
//使用消息类，实例化一个变量
ros::init(argc, argv, "publisher");
ros::NodeHandle h;
ros::Publisher puber = h.advertise<top::Name>("Name_Info", 10);
//实例化发布器类，创建且使用<包名::消息>的发布"Name_Info"这个话题
ros::Rate loop_rate(1);
ss << "A B";
// 基于C++编程，不解释
while (ros::ok())
{
ss >> msg.first_name;
// 基于C++编程，不解释
ss >> msg.last_name;
// 基于C++编程，不解释
ROS_INFO("I Send [%s %s]", msg.first_name.c_str(), msg.last_name.c_str());
// 在控制台上显示信息，使用ROS_INFO，可以查看日志输出来复原操作
puber.publish(msg);
loop_rate.sleep();
}
return 0;
}
订阅节点
#include "ros/ros.h"
#include "top/Name.h"
void infoCallback(const top::Name::ConstPtr& msg)
// 回调函数名(常量 包名::消息::常量指针 变量名), 用于接收消息，且防止被修改
{
ROS_INFO("Name Changed [%s %s]", msg->last_name.c_str(), msg->first_name.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "suber");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("Name_Info", 1000, infoCallback);
// 实例化订阅器，且订阅"Name_Info"这个话题，并调用回调函数infoCallback
ros::spin();
// 有回调函数记得加上这句话
return 0;
}
②Python版
发布节点
#!/usr/bin/env python
import rospy
from egpy.msg import Name #
def talker():
pub = rospy.Publisher('Name_Info', Name, queue_size=10) # 实例化一个发布者
rospy.init_node('pyPub', anonymous=True) # 初始化节点信息
rate = rospy.Rate(10)
a = Name() # 实例化消息
while not rospy.is_shutdown():
a.first_name = "A"
a.last_name = "B"
rospy.loginfo("%s %s"%(a.first_name, a.last_name)) # 等同ROS_INFO
pub.publish(a)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
订阅节点
#!/usr/bin/env python
import rospy
from egpy.msg import Name
def callback(data):
rospy.loginfo("pyname:%s %s"%(data.first_name, data.last_name))
def listener():
rospy.init_node('pySub', anonymous=True)
rospy.Subscriber("Name_Info", Name, callback)
rospy.spin()
if __name__ == '__main__':
listener()
2.服务篇
①C++版
服务端
#include "ros/ros.h"
#include "server/add.h"
//定义服务的类型 包名/服务文件.h
bool add(server::add::Request &req, server::add::Response &res)
//定义服务的回调函数，且请求与响应是同一个文件里的不同两类，所以分开声明
{
res.sum = req.a + req.b;
ROS_INFO("Get request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ROS_INFO("The server is start running!");
ros::ServiceServer service = n.advertiseService("add", add);
//实例化服务
ros::spin();
// spin函数是永远执行回调函数，而不管它以下的代码，所以要放至末尾
return 0;
}
客户端
#include "ros/ros.h"
#include "server/add.h"
#include ②Python版
服务端
#!/usr/bin/env python
from serpy.srv import *
import rospy
def handle_add_two_ints(req): # 服务的回调函数
print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
return addResponse(req.a + req.b)
def add_two_ints_server():
rospy.init_node('add_server')
s = rospy.Service('add', add, handle_add_two_ints)
# 实例化服务端，并设置服务名，数据类型与回调函数
print "Ready to add two ints."
rospy.spin() # 进入死循环只处理回调
if __name__ == "__main__":
add_two_ints_server()
客户端
#!/usr/bin/env python
import sys
import rospy
import random
from serpy.srv import *
def sendrequest(x, y):
rospy.wait_for_service('add')
# 等待服务端启动，会阻塞在这里，直到有add这个服务出现
try:
req = rospy.ServiceProxy('add', add) # 实例化一个请求
resp1 = req(x, y) # 进行请求并返回
return resp1.sum
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def runforever():
rospy.init_node('add_client', anonymous=True)
rate = rospy.Rate(1) # 控制一秒一个请求
while not rospy.is_shutdown():
x = int(random.random()*100)
y = int(random.random()*100)
print "Requesting %d+%d"%(x, y)
print "%s + %s = %s"%(x, y, sendrequest(x, y))
rate.sleep()
if __name__ == "__main__":
runforever()
三.验证
我们的Python节点和C++节点是可以共存的，就像这样：
