ROS学习9:ROS进阶

【Autolabor初级教程】ROS机器人入门

1. action 通信

  • 背景

    • 机器人导航到某个目标点,此过程需要一个节点 A 发布目标信息,然后一个节点 B 接收到请求并控制移动,最终响应目标达成状态信息。乍一看好像是服务通信实现,因为需要 A 发送目标,B 执行并返回结果,这是一个典型的基于请求响应的应答模式
    • 但是,如果只是使用基本的服务通信实现,存在一个问题:导航是一个耗时过程,如果使用服务通信,则只有在导航结束时,才会产生响应结果,而在导航过程中,节点 A 获取不到任何反馈,从而可能出现程序 “假死” 现象,过程的不可控意味着不良的用户体验,以及逻辑处理的缺陷(比如:导航中止的需求无法实现
    • 更合理的方案:导航过程中,可以连续反馈当前机器人状态信息,当导航终止时,再返回最终的执行结果。在 ROS 中,该实现策略称之为:action 通信
  • 概念

    • ROS 中提供了 actionlib 功能包集,用于实现 action 通信。action 是一种类似于服务通信的实现,其实现模型也包含请求和响应,但不同的是:在请求和响应的过程中,服务端还可以连续的反馈当前任务进度,客户端可以接收连续反馈并且还可以取消任务

    • action 结构图解
      ROS学习9:ROS进阶_第1张图片

    • action 通信接口图解
      ROS学习9:ROS进阶_第2张图片

      • goal:目标任务
      • cacel:取消任务
      • status:服务端状态
      • result:最终执行结果(只会发布一次)
      • feedback:连续反馈(可以发布多次)
  • 作用

    • 一般适用于耗时的请求响应场景,用以获取连续的状态反馈

1.1 自定义 action 文件

  • 定义 action 文件

    • 创建工作空间

      $ mkdir -p demo06_ws/src
      $ cd demo06_ws
      $ catkin_make
      
      $ code .
      
    • 右键 src 创建功能包 demo6_ws,并导入依赖: roscpp rospy std_msgs actionlib actionlib_msgs

    • 功能包下新建 action 目录,然后新建 AddInts.action 文件

      # 目标值
      int32 num
      ---
      # 最终结果
      int32 result
      ---
      # 连续反馈
      float64 progress_bar
      
  • 编辑配置文件 CMakeLists.txt

    find_package(catkin REQUIRED COMPONENTS
      actionlib
      actionlib_msgs
      roscpp
      rospy
      std_msgs
    )
    
    ## Generate actions in the 'action' folder
    add_action_files(
      FILES
      AddInts.action
    )
    
    ## Generate added messages and services with any dependencies listed here
    generate_messages(
      DEPENDENCIES
      actionlib_msgs
      std_msgs
    )
    
    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES demo01_action
     CATKIN_DEPENDS actionlib actionlib_msgs roscpp rospy std_msgs
    #  DEPENDS system_lib
    )
    

1.2 action 通信实现(C++)

  • 需求

    • 创建两个 ROS 节点:服务器和客户端,其中客户端可以向服务器发送目标数据 N (int 类型),服务器会计算 1 到 N 之间所有整数的和(这是一个循环累加的过程),然后返回给客户端,这是基于请求响应模式。又已知服务器从接收到请求到产生响应是一个耗时操作,每累加一次耗时 0.1s,为了良好的用户体验,需要服务器在计算过程中,每累加一次,就给客户端响应一次百分比格式的执行进度,使用 action 实现
  • 实现流程

  • 1. vscode 配置

    • 配置 c_cpp_properies.json 文件
    {
        "configurations": [
            {
                "browse": {
                    "databaseFilename": "",
                    "limitSymbolsToIncludedHeaders": true
                },
                "includePath": [
                    "/opt/ros/noetic/include/**",
                    "/usr/include/**",
                    "/home/yxd/ros_test/demo06_ws/devel/include/**" // 配置 head 文件的路径 
                ],
                "name": "ROS",
                "intelliSenseMode": "gcc-x64",
                "compilerPath": "/usr/bin/gcc",
                "cStandard": "c11",
                "cppStandard": "c++17"
            }
        ],
        "version": 4
    }
    
  • 2. 服务端实现

    // action01_server.cpp
    #include "ros/ros.h"
    #include "actionlib/server/simple_action_server.h"
    #include "demo01_action/AddIntsAction.h"
    
    typedef actionlib::SimpleActionServer<demo01_action::AddIntsAction> Server;
    
    // 请求处理(a.解析提交的目标值;b.产生连续反馈;c.最终结果响应) ---回调函数
    void cb(const demo01_action::AddIntsGoalConstPtr &goal, Server* server){
        // a.解析提交的目标值
        int num = goal->num;
        ROS_INFO("目标值:%d", num);
    
        // b.产生连续反馈
        int result = 0;
        ros::Rate rate(10); // 通过频率设置休眠时间
        for (int i = 1; i <= num; i++) {
            // 累加
            result += i;
            // 产生连续反馈,组织连续数据并发布
            demo01_action::AddIntsFeedback feedback;
            feedback.progress_bar = i / (double)num;
            server->publishFeedback(feedback);
    
            rate.sleep();
        }
    
        // c.最终结果响应
        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 execute_callback, 
                            bool auto_start)
            参数 1:NodeHandle
            参数 2:话题名称
            参数 3:回调函数
            参数 4:是否自动启动
        */
        // actionlib::SimpleActionServer server(....);
        Server server(nh, "addInts", boost::bind(&cb, _1, &server), false);
        server.start();  // 若上行代码自动启动设置为 false,则需手动调用 start 函数启动服务
        // 5.处理请求,产生反馈与响应;
    
        // 6.spin().   
        ros::spin();
        return 0;
    }
    
  • 3. 客户端实现

    // action02_client.cpp
    #include "ros/ros.h"
    #include "actionlib/client/simple_action_client.h"
    #include "demo01_action/AddIntsAction.h"
    
    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 client(nh,"addInts");
        Client client(nh,"addInts",true);
        //等待服务启动
        client.waitForServer();
        // 5.发送目标,处理反馈以及最终结果;
        /*  
            void sendGoal(const demo01_action::AddIntsGoal &goal, 
                boost::function done_cb, 
                boost::function active_cb, 
                boost::function feedback_cb)
    
        */
        demo01_action::AddIntsGoal goal;
        goal.num = 10;
    
        client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
        // 6.spin().
        ros::spin();
        return 0;
    }
    
  • 4. 编辑配置文件

    add_executable(action01_server src/action01_server.cpp)
    add_executable(action02_client src/action02_client.cpp)
    
    add_dependencies(action01_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
    add_dependencies(action02_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
    
    target_link_libraries(action01_server
      ${catkin_LIBRARIES}
    )
    target_link_libraries(action02_client
      ${catkin_LIBRARIES}
    )
    
  • 5. 执行并查看结果

    $ roscore
    
    # 服务端启动
    $ source devel/setup.bash 
    $ rosrun demo01_action action01_server 
    [ INFO] [1697618181.712584129]: action服务端实现
    [ INFO] [1697618204.874229874]: 目标值:10
    [ INFO] [1697618205.877111637]: 最终结果:55
    
    # 客户端启动
    $ source devel/setup.bash 
    $ rosrun demo01_action action02_client 
    [ INFO] [1697618204.874318234]: 服务已经被激活....
    [ INFO] [1697618204.876057963]: 当前进度:0.10
    [ INFO] [1697618204.979878251]: 当前进度:0.20
    [ INFO] [1697618205.088223356]: 当前进度:0.30
    [ INFO] [1697618205.177005844]: 当前进度:0.40
    [ INFO] [1697618205.276733595]: 当前进度:0.50
    [ INFO] [1697618205.384083330]: 当前进度:0.60
    [ INFO] [1697618205.480721833]: 当前进度:0.70
    [ INFO] [1697618205.578354465]: 当前进度:0.80
    [ INFO] [1697618205.677074995]: 当前进度:0.90
    [ INFO] [1697618205.776672717]: 当前进度:1.00
    [ INFO] [1697618205.877300477]: 最终结果:55
    

2. 动态参数

  • 参数服务器的数据被修改时,如果节点不重新访问,那么就不能获取修改后的数据

    • 例如:在乌龟背景色修改的案例中,先启动乌龟显示节点,然后再修改参数服务器中关于背景色设置的参数,那么窗体的背景色是不会修改的,必须要重启乌龟显示节点才能生效
  • 一些特殊场景下,要求做到参数动态获取,也即参数一旦修改,能够通知节点参数已经修改并读取修改后的数据

    • 机器人调试时,需要修改机器人轮廓信息(长宽高)、传感器位姿信息…,如果这些信息存储在参数服务器中,那么意味着需要重启节点,才能使更新设置生效,但是希望修改完毕之后,某些节点能够即时更新这些参数信息
  • 在 ROS 中针对这种场景已经给出的解决方案:dynamic reconfigure 动态配置参数

    • 动态配置参数,之所以能够实现即时更新,因为被设计成 CS 架构,客户端修改参数就是向服务器发送请求,服务器接收到请求之后,读取的是修改后的参数

2.1 动态参数客户端

  • 需求

    • 编写两个节点,一个节点可以动态修改参数,另一个节点时时解析修改后的数据
  • 1. 新建功能包

    • 新建功能包 demo02_dr,添加依赖:roscpp rospy std_msgs dynamic_reconfigure
  • 2. 新建 cfg 文件夹并添加 dr.cfg 文件

    #! /usr/bin/env python
    
    # 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, "int", 50, 0, 100)
    gen.add("double_param", double_t, 0, "double", 1.57, 0, 3.14)
    gen.add("string_param", str_t, 0, "string", "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, "list", 0, 0, 2, edit_method = many_enum)
    
    # 4.生成中间文件并退出
    exit(gen.generate(PACKAGE, "dr_node", "dr"))
    
  • 3. 配置 CMakeLists.txt

    generate_dynamic_reconfigure_options(
      cfg/dr.cfg
    )
    
  • 4. 编译

    • 编译后会在 devel/include 和 devel/lib 文件夹下生成中间文件

2.2 动态参数服务端

  • 1. 服务器代码实现

    // demo01_dr_server
    #include "ros/ros.h"
    #include "dynamic_reconfigure/server.h"
    #include "demo02_dr/drConfig.h"
    
    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;
    }
    
  • 2. 编辑配置文件

    add_executable(demo01_dr_server src/demo01_dr_server.cpp)
    ...
    
    add_dependencies(demo01_dr_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
    ...
    
    target_link_libraries(demo01_dr_server
      ${catkin_LIBRARIES}
    )
    
  • 3. 执行

    $ roscore
    
    $ rosrun demo02_dr demo01_dr_server
    
    $ rosrun rqt_reconfigure rqt_reconfigure
    

3. pluginlib

  • pluginlib:插件库,所谓插件就是可插拔的组件

    • 以计算机为例,可以通过 USB 接口自由插拔的键盘、鼠标、U 盘等都可以看作是插件实现,其基本原理就是通过规范化的 USB 接口协议实现计算机与 USB 设备的自由组合
    • 在软件编程中,插件是一种遵循一定规范的应用程序接口编写出来的程序,插件程序依赖于某个应用程序,且应用程序可以与不同的插件程序自由组合。在 ROS 中,也会经常使用到插件,场景如下
      • 导航插件:在导航中,涉及到路径规划模块,路径规划算法有多种,也可以自实现,导航应用时,可能需要测试不同算法的优劣以选择更合适的实现,这种场景下,ROS 就是通过插件的方式来实现不同算法的
      • rviz 插件:在 rviz 中已经提供了丰富的功能实现,但是即便如此,特定场景下,开发者可能需要实现某些定制化功能并集成到 rviz 中,这一集成过程也是基于插件的
  • 概念

    • pluginlib 是一个 c++ 库, 用来从一个 ROS 功能包中加载和卸载插件 (plugin)
    • 插件是指从运行时库中动态加载的类,通过使用 Pluginlib,不必将某个应用程序显式地链接到包含某个类的库,Pluginlib 可以随时打开包含类的库,而不需要应用程序事先知道包含类定义的库或者头文件
  • 作用

    • 结构清晰
    • 低耦合,易修改,可维护性强
    • 可移植性强,更具复用性
    • 结构容易调整,插件可以自由增减

3.1 pluginlib 使用

  • 需求

    • 以插件的方式实现正多边形的相关计算
  • 1. 准备工作

    • 创建功能包 demo03_plugin 导入依赖:roscpp pluginlib
    • 在 VSCode 中需要配置 .vascode/c_cpp_properties.json 文件中关于 includepath 选项的设置
{
    "configurations": [
        {
            "browse": {
                "databaseFilename": "",
                "limitSymbolsToIncludedHeaders": true
            },
            "includePath": [
                "/opt/ros/noetic/include/**",
                "/usr/include/**",
                "/home/yxd/ros_test/demo07_ws/devel/include/**" //配置 head 文件的路径 
            ],
            "name": "ROS",
            "intelliSenseMode": "gcc-x64",
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "c++17"
        }
    ],
    "version": 4
}
  • 2. 创建基类

    • 在 demo03_plugin/include/demo03_plugin 下新建头文件:polygon_base.h,所有插件类都要继承此基类
    • 基类必须提供无参构造函数,所以关于多边形的边长没有通过构造函数而是通过单独编写的 initialize 函数传参
    // polygon_base.h
    #ifndef POLYGON_BASE_H_
    #define 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
    
  • 3. 创建插件(类)

    • 在 demo03_plugin/include/demo03_plugin 下新建头文件:polygon_plugins.h
    #ifndef POLYGON_PLUGINS_H_
    #define POLYGON_PLUGINS_H_
    
    #include 
    #include 
    
    // 创建了正方形与三角形两个派生类继承基类
    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
    
  • 4. 注册插件(类)

    • 在 src 目录下新建 polygon_plugins.cpp 文件
    #include   // pluginlib 宏,可以注册插件类
    #include 
    #include 
    
    // 参数 1:派生类 参数 2:基类
    PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)
    PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
    
  • 5. 构建插件库

    • 在 CMakeLists.txt 文件中设置内容如下
    include_directories(
      include
      ${catkin_INCLUDE_DIRS}
    )
    add_library(polygon_plugins 
      src/polygon_plugins.cpp
    )
    

至此,可调用 catkin_make 编译,编译完成后,在工作空间 /devel/lib 目录下,会生成相关的 .so 动态链接库文件

  • 6. 使插件(类)可用于 ROS 工具链
  • 6.1 配置 xml
    • 功能包下新建文件:polygon_plugins.xml
    
    <library path = "lib/libpolygon_plugins">
        
        <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>
    
  • 6.2 导出插件
    • package.xml 文件中设置内容如下
    <export>
        
        <demo03_plugin plugin = "${prefix}/polygon_plugins.xml" />
    export>
    

编译后,可以调用 rospack plugins --attrib=plugin demo03_plugin 命令查看配置是否正常,如无异常,会返回 6.1 中编写的 polygon_plugins.xml 文件的完整路径,这意味着插件已经正确的集成到了 ROS 工具链

  • 7. 使用插件
    • src 下新建 c++ 文件:polygon_loader.cpp
// polygon_loader.cpp
#include   // 类加载器相关的头文件
#include 

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;
}
  • 8. 执行
    • 修改 CMakeLists.txt 文件
    add_executable(polygon_loader src/polygon_loader.cpp)
    
    target_link_libraries(polygon_loader
        ${catkin_LIBRARIES}
    )
    
    $ source ./devel/setup.bash
    $ rosrun demo03_plugin polygon_loader
    
    [ INFO] [WallTime: 1279658450.869089666]: Triangle area: 43.30
    [ INFO] [WallTime: 1279658450.869138007]: Square area: 100.00
    

4. nodelet

  • ROS 通信是基于 Node (节点) 的,Node 使用方便、易于扩展,可以满足 ROS 中大多数应用场景,但是也存在一些局限性,由于一个 Node 启动之后独占一根进程,不同 Node 之间数据交互其实是不同进程之间的数据交互,当传输类似于图片、点云的大容量数据时,会出现延时与阻塞的情况

    • 现在需要编写一个相机驱动,在该驱动中有两个节点实现:其中节点 A 负责发布原始图像数据,节点 B 订阅原始图像数据并在图像上标注人脸。如果节点 A 与节点 B 仍按照之前实现,两个节点分别对应不同的进程,在两个进程之间传递容量可观图像数据,可能就会出现延时的情况
    • ROS 中给出的解决方案是:Nodelet,通过 Nodelet 可以将多个节点集成进一个进程
  • 概念

    • nodelet 软件包旨在提供在同一进程中运行多个算法 (节点) 的方式,不同算法之间通过传递指向数据的指针来代替了数据本身的传输 (类似于传值与传址的区别),从而实现零成本的数据拷贝
    • nodelet 功能包的核心实现也是插件,是对插件的进一步封装
      • 不同算法被封装进插件类,可以像单独的节点一样运行
      • 在该功能包中提供插件类实现的基类:Nodelet
      • 并且提供了加载插件类的类加载器:NodeletLoader
  • 作用

    • 应用于大容量数据传输的场景,提高节点间的数据交互效率,避免延时与阻塞

4.1 内置案例演示

  • 1. 案例简介

    • 在 ROS 中内置了 nodelet 案例,在该案例中,定义了一个 Nodelet 插件类:Plus,这个节点可以订阅一个数字,并将订阅到的数字与参数服务器中的 value 参数相加后再发布
    • 需求:在同一线程中启动两个 Plus 节点 A 与 B,向 A 发布一个数字,然后经 A 处理后,再发布并作为 B 的输入,最后打印 B 的输出
  • 2. nodelet 基本使用语法

    nodelet load pkg/Type manager - Launch a nodelet of type pkg/Type on manager manager
    nodelet standalone pkg/Type   - Launch a nodelet of type pkg/Type in a standalone node
    nodelet unload name manager   - Unload a nodelet a nodelet by name from manager
    nodelet manager               - Launch a nodelet manager node
    
  • 3. 内置案例调用

    
    <launch>
        
        <node pkg = "nodelet" type = "nodelet" name = "mymanager" args = "manager" output = "screen" />
        
        <node pkg = "nodelet" type = "nodelet" name = "n1" args = "load nodelet_tutorial_math/Plus mymanager" output = "screen">
            <param name = "value" value = "100" />
        node>
        
        <node pkg = "nodelet" type = "nodelet" name = "n2" args = "load nodelet_tutorial_math/Plus mymanager" output = "screen">
            <param name = "value" value = "-50" />
            
            <remap from = "/n2/in" to = "/n1/out" />
        node>
    launch>
    
    $ source ./devel/setup.bash
    $ rosrun demo04_nodelet test01_nodelet.launch
    
  • 4. 执行

    • 向节点 n1 发布消息
    $ rostopic pub -r 10 /n1/in std_msgs/Float64 "data: 10.0"
    
    • 打印节点 n2 发布的消息
    $ rostopic echo /n2/out
    data: 60.0
    ---
    ...
    

4.2 自定义 nodelet 实现

  • nodelet 本质也是插件,实现流程与插件实现流程类似,并且不需要自定义接口,也不需要使用类加载器加载插件类

  • 需求

    • 参考 nodelet 案例,编写 nodelet 插件类,可以订阅输入数据,设置参数,发布订阅数据与参数相加的结果
  • 1. 准备

    • 新建功能包,导入依赖:roscpp、nodelet
  • 2. 创建插件类并注册插件

    #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)
    
  • 3. 构建插件库

    • CMakeLists.txt 配置如下
    add_library(mynodeletlib
      src/myplus.cpp
    )
    
    target_link_libraries(mynodeletlib
      ${catkin_LIBRARIES}
    )
    

编译后,会在工作空间 /devel/lib/ 生成文件:libmynodeletlib.so

  • 4. 使插件可用于 ROS 工具链

  • 4.1 配置 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>hellodescription>
        class>
    library>
    
  • 4.2 导出插件

    • package.xml 文件中设置内容如下
    <export>
        
        <nodelet plugin = "${prefix}/my_plus.xml" />
    export>
    
  • 5. 执行

    • 通过 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>
    
    • 向节点 n1 发布消息
    $ rostopic pub -r 10 /p1/in std_msgs/Float64 "data: 10.0"
    
    • 打印节点 n2 发布的消息
    $ rostopic echo /p2/out
    data: 60.0
    ---
    ...
    

你可能感兴趣的:(ROS学习,学习,ROS,笔记,action通信,nodelet,pluginlib,动态参数)