ROS学习9：ROS进阶

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

1. action 通信

• 背景

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

• 概念

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

  • action 结构图解
    

  • action 通信接口图解
    

    • 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
  ---
  ...