目录
action通信
关于action通信,我们先从之前导航中的应用场景开始介绍,描述如下:
机器人导航到某个目标点,此过程需要一个节点A发布目标信息,然后一个节点B接收到请求并控制移动,最终响应目标达成状态信息。
乍一看,这好像是服务通信实现,因为需求中要A发送目标,B执行并返回结果,这是一个典型的基于请求响应的应答模式,不过,如果只是使用基本的服务通信实现,存在一个问题:导航是一个过程,是耗时操作,如果使用服务通信,那么只有在导航结束时,才会产生响应结果,而在导航过程中,节点A是不会获取到任何反馈的,从而可能出现程序"假死"的现象,过程的不可控意味着不良的用户体验,以及逻辑处理的缺陷(比如:导航中止的需求无法实现)。更合理的方案应该是:导航过程中,可以连续反馈当前机器人状态信息,当导航终止时,再返回最终的执行结果。在ROS中,该实现策略称之为:action 通信。
概念
在ROS中提供了actionlib功能包集,用于实现 action 通信。action 是一种类似于服务通信的实现,其实现模型也包含请求和响应,但是不同的是,在请求和响应的过程中,服务端还可以连续的反馈当前任务进度,客户端可以接收连续反馈并且还可以取消任务。
作用
一般适用于耗时的请求响应场景,用以获取连续的状态反馈。
action通信自定义action文件
定义action文件首先新建功能包,并导入依赖: roscpp rospy std_msgs actionlib actionlib_msgs;
然后功能包下新建 action 目录,新增 Xxx.action(比如:AddInts.action)。
action 文件内容组成分为三部分:请求目标值、最终响应结果、连续反馈,三者之间使用---分割示例内容如下:
#目标值 int32 num --- #最终结果 int32 result --- #连续反馈 float64 progress_bar
编辑配置文件
find_package (catkin REQUIRED COMPONENTS roscpp rospy std_msgs actionlib actionlib_msgs ) add_action_files( FILES AddInts.action ) generate_messages( DEPENDENCIES std_msgs actionlib_msgs ) catkin_package( # INCLUDE_DIRS include # LIBRARIES demo04_action CATKIN_DEPENDS roscpp rospy std_msgs actionlib actionlib_msgs # DEPENDS system_lib )
服务端代码
#include "ros/ros.h" #include "actionlib/server/simple_action_server.h" #include "demo01_action/AddIntsAction.h" /* 需求: 创建两个ROS节点,服务器和客户端, 客户端可以向服务器发送目标数据N(一个整型数据) 服务器会计算1到N之间所有整数的和,这是一个循环累加的过程,返回给客户端, 这是基于请求响应模式的, 又已知服务器从接收到请求到产生响应是一个耗时操作,每累加一次耗时0.1s, 为了良好的用户体验,需要服务器在计算过程中, 每累加一次,就给客户端响应一次百分比格式的执行进度,使用action实现。 流程: 1.包含头文件; 2.初始化ROS节点; 3.创建NodeHandle; 4.创建action服务对象; 5.处理请求,产生反馈与响应; 6.spin(). */ typedef actionlib::SimpleActionServer<demo01_action::AddIntsAction> Server; void cb(const demo01_action::AddIntsGoalConstPtr &goal,Server* server){ //获取目标值 int num = goal->num; ROS_INFO("目标值:%d",num); //累加并响应连续反馈 int result = 0; demo01_action::AddIntsFeedback feedback;//连续反馈 ros::Rate rate(10);//通过频率设置休眠时间 for (int i = 1; i <= num; i++) { result += i; //组织连续数据并发布 feedback.progress_bar = i / (double)num; server->publishFeedback(feedback); rate.sleep(); } //设置最终结果 demo01_action::AddIntsResult r; r.result = result; server->setSucceeded(r); ROS_INFO("最终结果:%d",r.result); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); ROS_INFO("action服务端实现"); // 2.初始化ROS节点; ros::init(argc,argv,"AddInts_server"); // 3.创建NodeHandle; ros::NodeHandle nh; // 4.创建action服务对象; /*SimpleActionServer(ros::NodeHandle n, std::string name, boost::function<void (const demo01_action::AddIntsGoalConstPtr &)> execute_callback, bool auto_start) */ // actionlib::SimpleActionServer<demo01_action::AddIntsAction> server(....); Server server(nh,"addInts",boost::bind(&cb,_1,&server),false); server.start(); // 5.处理请求,产生反馈与响应; // 6.spin(). ros::spin(); return 0; }
客户端代码
#include "ros/ros.h" #include "actionlib/client/simple_action_client.h" #include "demo01_action/AddIntsAction.h" /* 需求: 创建两个ROS节点,服务器和客户端, 客户端可以向服务器发送目标数据N(一个整型数据) 服务器会计算1到N之间所有整数的和,这是一个循环累加的过程,返回给客户端, 这是基于请求响应模式的, 又已知服务器从接收到请求到产生响应是一个耗时操作,每累加一次耗时0.1s, 为了良好的用户体验,需要服务器在计算过程中, 每累加一次,就给客户端响应一次百分比格式的执行进度,使用action实现。 流程: 1.包含头文件; 2.初始化ROS节点; 3.创建NodeHandle; 4.创建action客户端对象; 5.发送目标,处理反馈以及最终结果; 6.spin(). */ typedef actionlib::SimpleActionClient<demo01_action::AddIntsAction> Client; //处理最终结果 void done_cb(const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result){ if (state.state_ == state.SUCCEEDED) { ROS_INFO("最终结果:%d",result->result); } else { ROS_INFO("任务失败!"); } } //服务已经激活 void active_cb(){ ROS_INFO("服务已经被激活...."); } //处理连续反馈 void feedback_cb(const demo01_action::AddIntsFeedbackConstPtr &feedback){ ROS_INFO("当前进度:%.2f",feedback->progress_bar); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化ROS节点; ros::init(argc,argv,"AddInts_client"); // 3.创建NodeHandle; ros::NodeHandle nh; // 4.创建action客户端对象; // SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true) // actionlib::SimpleActionClient<demo01_action::AddIntsAction> client(nh,"addInts"); Client client(nh,"addInts",true); //等待服务启动 client.waitForServer(); // 5.发送目标,处理反馈以及最终结果; /* void sendGoal(const demo01_action::AddIntsGoal &goal, boost::function<void (const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result)> done_cb, boost::function<void ()> active_cb, boost::function<void (const demo01_action::AddIntsFeedbackConstPtr &feedback)> feedback_cb) */ demo01_action::AddIntsGoal goal; goal.num = 10; client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb); // 6.spin(). ros::spin(); return 0; }
动态参数
概念
一种可以在运行时更新参数而无需重启节点的参数配置策略。
参数服务器的数据被修改时,如果节点不重新访问,那么就不能获取修改后的数据,例如在乌龟背景色修改的案例中,先启动乌龟显示节点,然后再修改参数服务器中关于背景色设置的参数,那么窗体的背景色是不会修改的,必须要重启乌龟显示节点才能生效。而一些特殊场景下,是要求要能做到动态获取的,也即,参数一旦修改,能够通知节点参数已经修改并读取修改后的数据,比如:
机器人调试时,需要修改机器人轮廓信息(长宽高)、传感器位姿信息....,如果这些信息存储在参数服务器中,那么意味着需要重启节点,才能使更新设置生效,但是希望修改完毕之后,某些节点能够即时更新这些参数信息。
在ROS中针对这种场景已经给出的解决方案: dynamic reconfigure 动态配置参数。
动态配置参数,之所以能够实现即时更新,因为被设计成 CS 架构,客户端修改参数就是向服务器发送请求,服务器接收到请求之后,读取修改后的是参数。
作用
主要应用于需要动态更新参数的场景,比如参数调试、功能切换等。典型应用:导航时参数的动态调试。
动态参数客户端
客户端实现流程:
- 新建并编辑 .cfg 文件;
- 编辑CMakeLists.txt;
- 编译。
添加.cfg文件
新建 cfg 文件夹,添加 xxx.cfg 文件(并添加可执行权限),cfg 文件其实就是一个 python 文件,用于生成参数修改的客户端(GUI)。
#! /usr/bin/env python """ 4生成动态参数 int,double,bool,string,列表 5实现流程: 6 1.导包 7 2.创建生成器 8 3.向生成器添加若干参数 9 4.生成中间文件并退出 10 """ # 1.导包 from dynamic_reconfigure.parameter_generator_catkin import * PACKAGE = "demo02_dr" # 2.创建生成器 gen = ParameterGenerator() # 3.向生成器添加若干参数 #add(name, paramtype, level, description, default=None, min=None, max=None, edit_method="") gen.add("int_param",int_t,0,"整型参数",50,0,100) gen.add("double_param",double_t,0,"浮点参数",1.57,0,3.14) gen.add("string_param",str_t,0,"字符串参数","hello world ") gen.add("bool_param",bool_t,0,"bool参数",True) many_enum = gen.enum([gen.const("small",int_t,0,"a small size"), gen.const("mediun",int_t,1,"a medium size"), gen.const("big",int_t,2,"a big size") ],"a car size set") gen.add("list_param",int_t,0,"列表参数",0,0,2, edit_method=many_enum) # 4.生成中间文件并退出 exit(gen.generate(PACKAGE,"dr_node","dr"))
chmod +x xxx.cfg添加权限
配置 CMakeLists.txt
generate_dynamic_reconfigure_options( cfg/mycar.cfg )
动态参数服务端A(C++)
#include "ros/ros.h" #include "dynamic_reconfigure/server.h" #include "demo02_dr/drConfig.h" /* 动态参数服务端: 参数被修改时直接打印 实现流程: 1.包含头文件 2.初始化 ros 节点 3.创建服务器对象 4.创建回调对象(使用回调函数,打印修改后的参数) 5.服务器对象调用回调对象 6.spin() */ void cb(demo02_dr::drConfig& config, uint32_t level){ ROS_INFO("动态参数解析数据:%d,%.2f,%d,%s,%d", config.int_param, config.double_param, config.bool_param, config.string_param.c_str(), config.list_param ); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ros 节点 ros::init(argc,argv,"dr"); // 3.创建服务器对象 dynamic_reconfigure::Server<demo02_dr::drConfig> server; // 4.创建回调对象(使用回调函数,打印修改后的参数) dynamic_reconfigure::Server<demo02_dr::drConfig>::CallbackType cbType; cbType = boost::bind(&cb,_1,_2); // 5.服务器对象调用回调对象 server.setCallback(cbType); // 6.spin() ros::spin(); return 0; }
pluginlib
概念
pluginlib直译是插件库,所谓插件字面意思就是可插拔的组件,比如:以计算机为例,可以通过USB接口自由插拔的键盘、鼠标、U盘...都可以看作是插件实现,其基本原理就是通过规范化的USB接口协议实现计算机与USB设备的自由组合。同理,在软件编程中,插件是一种遵循一定规范的应用程序接口编写出来的程序,插件程序依赖于某个应用程序,且应用程序可以与不同的插件程序自由组合。在ROS中,也会经常使用到插件,场景如下:
1.导航插件:在导航中,涉及到路径规划模块,路径规划算法有多种,也可以自实现,导航应用时,可能需要测试不同算法的优劣以选择更合适的实现,这种场景下,ROS中就是通过插件的方式来实现不同算法的灵活切换的。
2.rviz插件:在rviz中已经提供了丰富的功能实现,但是即便如此,特定场景下,开发者可能需要实现某些定制化功能并集成到rviz中,这一集成过程也是基于插件的。
作用
- 结构清晰;
- 低耦合,易修改,可维护性强;
- 可移植性强,更具复用性;
- 结构容易调整,插件可以自由增减;
实现流程:
- 准备;
- 创建基类;
- 创建插件类;
- 注册插件;
- 构建插件库;
- 使插件可用于ROS工具链;
-
- 配置xml
- 导出插件
- 使用插件;
- 执行。
创建基类
在 xxx/include/xxx下新建C++头文件: polygon_base.h,所有的插件类都需要继承此基类,内容如下:
#ifndef XXX_POLYGON_BASE_H_ #define XXX_POLYGON_BASE_H_ namespace polygon_base { class RegularPolygon { public: virtual void initialize(double side_length) = 0; virtual double area() = 0; virtual ~RegularPolygon(){} protected: RegularPolygon(){} }; }; #endif
PS:基类必须提供无参构造函数,所以关于多边形的边长没有通过构造函数而是通过单独编写的initialize函数传参。
创建插件
在 xxx/include/xxx下新建C++头文件:polygon_plugins.h,内容如下:
#ifndef XXX_POLYGON_PLUGINS_H_ #define XXX_POLYGON_PLUGINS_H_ #include <xxx/polygon_base.h> #include <cmath> namespace polygon_plugins { class Triangle : public polygon_base::RegularPolygon { public: Triangle(){} void initialize(double side_length) { side_length_ = side_length; } double area() { return 0.5 * side_length_ * getHeight(); } double getHeight() { return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2))); } private: double side_length_; }; class Square : public polygon_base::RegularPolygon { public: Square(){} void initialize(double side_length) { side_length_ = side_length; } double area() { return side_length_ * side_length_; } private: double side_length_; }; }; #endif
注册插件
在 src 目录下新建 polygon_plugins.cpp 文件,内容如下:
//pluginlib 宏,可以注册插件类 #include <pluginlib/class_list_macros.h> #include <xxx/polygon_base.h> #include <xxx/polygon_plugins.h> //参数1:衍生类 参数2:基类 PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon) PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
该文件会将两个衍生类注册为插件。
构建插件库
在 CMakeLists.txt 文件中设置内容如下:
include_directories(include) add_library(polygon_plugins src/polygon_plugins.cpp)
至此,可以调用 catkin_make 编译,编译完成后,在工作空间/devel/lib目录下,会生成相关的 .so 文件。
使插件可用于ROS工具链
配置xml
功能包下新建文件:polygon_plugins.xml,内容如下:
<!-- 插件库的相对路径 --> <library path="lib/libpolygon_plugins"> <!-- type="插件类" base_class_type="基类" --> <class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon"> <!-- 描述信息 --> <description>This is a triangle plugin.</description> </class> <class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon"> <description>This is a square plugin.</description> </class> </library>
导出插件
package.xml文件中设置内容如下:
<export> <xxx plugin="${prefix}/polygon_plugins.xml" /> </export>
标签<xxx />的名称应与基类所属的功能包名称一致,plugin属性值为上一步中创建的xml文件。
编译后,可以调用rospack plugins --attrib=plugin xxx命令查看配置是否正常,如无异常,会返回 .xml 文件的完整路径,这意味着插件已经正确的集成到了ROS工具链。
使用插件
src 下新建c++文件:polygon_loader.cpp,内容如下:
//类加载器相关的头文件 #include <pluginlib/class_loader.h> #include <xxx/polygon_base.h> int main(int argc, char** argv) { //类加载器 -- 参数1:基类功能包名称 参数2:基类全限定名称 pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("xxx", "polygon_base::RegularPolygon"); try { //创建插件类实例 -- 参数:插件类全限定名称 boost::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createInstance("polygon_plugins::Triangle"); triangle->initialize(10.0); boost::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createInstance("polygon_plugins::Square"); square->initialize(10.0); ROS_INFO("Triangle area: %.2f", triangle->area()); ROS_INFO("Square area: %.2f", square->area()); } catch(pluginlib::PluginlibException& ex) { ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); } return 0; }
执行
修改CMakeLists.txt文件,内容如下:
add_executable(polygon_loader src/polygon_loader.cpp) target_link_libraries(polygon_loader ${catkin_LIBRARIES})
Nodelet
概念
ROS通信是基于Node(节点)的,Node使用方便、易于扩展,可以满足ROS中大多数应用场景,但是也存在一些局限性,由于一个Node启动之后独占一根进程,不同Node之间数据交互其实是不同进程之间的数据交互,当传输类似于图片、点云的大容量数据时,会出现延时与阻塞的情况,比如:
现在需要编写一个相机驱动,在该驱动中有两个节点实现:其中节点A负责发布原始图像数据,节点B订阅原始图像数据并在图像上标注人脸。如果节点A与节点B仍按照之前实现,两个节点分别对应不同的进程,在两个进程之间传递容量可观图像数据,可能就会出现延时的情况,那么该如何优化呢?
ROS中给出的解决方案是:Nodelet,通过Nodelet可以将多个节点集成进一个进程。
nodelet软件包旨在提供在同一进程中运行多个算法(节点)的方式,不同算法之间通过传递指向数据的指针来代替了数据本身的传输(类似于编程传值与传址的区别),从而实现零成本的数据拷贝。
nodelet功能包的核心实现也是插件,是对插件的进一步封装:
- 不同算法被封装进插件类,可以像单独的节点一样运行;
- 在该功能包中提供插件类实现的基类:Nodelet;
- 并且提供了加载插件类的类加载器:NodeletLoader。
作用
应用于大容量数据传输的场景,提高节点间的数据交互效率,避免延时与阻塞。
流程说明
- 准备;
- 创建插件类并注册插件;
- 构建插件库;
- 使插件可用于ROS工具链;
- 执行。
准备
新建功能包,导入依赖: roscpp、nodelet;
创建插件类并注册插件
#include "nodelet/nodelet.h" #include "pluginlib/class_list_macros.h" #include "ros/ros.h" #include "std_msgs/Float64.h" namespace nodelet_demo_ns { class MyPlus: public nodelet::Nodelet { public: MyPlus(){ value = 0.0; } void onInit(){ //获取 NodeHandle ros::NodeHandle& nh = getPrivateNodeHandle(); //从参数服务器获取参数 nh.getParam("value",value); //创建发布与订阅对象 pub = nh.advertise<std_msgs::Float64>("out",100); sub = nh.subscribe<std_msgs::Float64>("in",100,&MyPlus::doCb,this); } //回调函数 void doCb(const std_msgs::Float64::ConstPtr& p){ double num = p->data; //数据处理 double result = num + value; std_msgs::Float64 r; r.data = result; //发布 pub.publish(r); } private: ros::Publisher pub; ros::Subscriber sub; double value; }; } PLUGINLIB_EXPORT_CLASS(nodelet_demo_ns::MyPlus,nodelet::Nodelet)
构建插件库
CMakeLists.txt配置如下:
... add_library(mynodeletlib src/myplus.cpp ) ... target_link_libraries(mynodeletlib ${catkin_LIBRARIES} )
编译后,会在 工作空间/devel/lib/先生成文件: libmynodeletlib.so。
使插件可用于ROS工具链
配置xml
新建 xml 文件,名称自定义(比如:my_plus.xml),内容如下:
<library path="lib/libmynodeletlib"> <class name="demo04_nodelet/MyPlus" type="nodelet_demo_ns::MyPlus" base_class_type="nodelet::Nodelet" > <description>hello</description> </class> </library>
导出插件
<export> <!-- Other tools can request additional information be placed here --> <nodelet plugin="${prefix}/my_plus.xml" /> </export>
执行
可以通过launch文件执行nodelet,示例内容如下:
<launch> <node pkg="nodelet" type="nodelet" name="my" args="manager" output="screen" /> <node pkg="nodelet" type="nodelet" name="p1" args="load demo04_nodelet/MyPlus my" output="screen"> <param name="value" value="100" /> <remap from="/p1/out" to="con" /> </node> <node pkg="nodelet" type="nodelet" name="p2" args="load demo04_nodelet/MyPlus my" output="screen"> <param name="value" value="-50" /> <remap from="/p2/in" to="con" /> </node> </launch>
运行launch文件,可以参考上一节方式向 p1发布数据,并订阅p2输出的数据,最终运行结果也与上一节类似。