Chapter7 ROS进阶

7.1 本章概述

第二章内容介绍了ROS的核心实现:通信机制 ——话题通信、服务通信和参数服务器。三者结合可以满足ROS中的大多数数据传输相关的应用场景，但是在一些特定场景下可能就有些力不从心了，本章主要介绍之前的通信机制存在的问题以及对应的优化策略，本章主要内容如下:

• action通信；
• 动态参数；
• pluginlib；
• nodelet。

本章预期达成的学习目标:

• 了解服务通信应用的局限性(action的应用场景)，熟练掌握action的理论模型与实现流程；
• 了解参数服务器应用的局限性(动态配置参数的应用场景)，熟练掌握动态配置参数的实现流程；
• 了解插件的概念以及使用流程；
• 了解nodelet的应用场景以及使用流程。

7.2 action通信

7.2.1 action通信的通信场景

1.应用场景

机器人导航到某个目标点,此过程需要一个节点A发布目标信息，然后一个节点B接收到请求并控制移动，最终响应目标达成状态信息。

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

2.概念

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

 

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

3.作用

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

4.案例

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

7.2.2 自定义action文件

1.简介

action、srv、msg 文件内的可用数据类型一致，且三者实现流程类似:

1. 按照固定格式创建action文件；

2. 编辑配置文件；

3. 编译生成中间文件。

2.具体实现流程（超详细版）

Ⅰ.新建功能包，导入依赖 roscpp std_msgs actionlib actionlib_msgs

Ⅱ.新建action目录，添加文件addints.action

#1.目标数据变量
int32 num
---
#2.最终响应变量
int32 result
---
#3.连续反馈变量
float64 progress_bar

Ⅲ.编辑配置文件

## 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 std_msgs
#  DEPENDS system_lib
)

Ⅳ.编译查看文件

7.2.3 action通信实现

1.流程

1. 编写action服务端实现；
2. 编写action客户端实现；
3. 编辑CMakeLists.txt；
4. 编译并执行。

2.VSCODE配置

需要像之前自定义 msg 实现一样配置c_cpp_properies.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同:

 

{
  "configurations": [
    {
      "browse": {
        "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
        "limitSymbolsToIncludedHeaders": false
      },
      "includePath": [
        "/opt/ros/melodic/include/**",
        "/home/liuhongwei/demo01_ws/src/helloworld1/include/**",
        "/usr/include/**",
        "/home/liuhongwei/demo06_ws/devel/include/**"    ---这是更改添加的位置
      ],
      "name": "ROS",
      "intelliSenseMode": "gcc-x64",
      "compilerPath": "/usr/bin/gcc",
      "cStandard": "gnu11",
      "cppStandard": "c++14"
    }
  ],
  "version": 4
}

3.服务端实现

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

    流程:
        1.包含头文件;
        2.初始化ROS节点;
        3.创建NodeHandle;
        4.创建action服务对象;
        5.处理请求,产生反馈与响应;
        6.spin().

Ⅱ.code

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "demo01_action/addintsAction.h"

typedef  actionlib::SimpleActionServer  Server;

void cb(const demo01_action::addintsGoalConstPtr & goalptr,Server *server)
{
    //解析提交的目标值
    int goal_num = goalptr->num;
    ROS_INFO("客户端提交的目标值是：%d",goal_num);
    
    //产生连续反馈
    ros::Rate rate(10);
    int result = 0;
    for (int i = 1; i < goal_num; i++)
    {
        result += i;
        rate.sleep();
        //void publishFeedback(const demo01_action::addintsFeedback & feedback)
        demo01_action::addintsFeedback fb;
        fb.progress_bar = i/(double)goal_num;
        server->publishFeedback(fb);
    }
    //最终结果响应
    demo01_action::addintsResult r;
    r.result = result;
    server->setSucceeded(r);

    
}

int main(int argc, char  *argv[])
{
    //初始化ros节点
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"add_server");

    //创建nodehandle
    ros::NodeHandle nh;

    //创建action服务对象
    //SimpleActionServer(ros::NodeHandle n,
    // std::__cxx11::string name, boost::function execute_callback, bool auto_start)
    // 1 nodehandle    2 话题    3.回调函数   4.是否自动启动
    Server server(nh,"addint",boost::bind(&cb,_1,&server),false);
    server.start();         //如果auto_start为false，则需手动调用该函数

    //spin
    ros::spin();
    return 0;
}

 Ⅲ.验证代码

①先配置cmakelist.txt

②编译

③查看节点及话题

liuhongwei@liuhongwei-virtual-machine:~/demo06_ws$ rosnode list 
/add_server
/rosout
liuhongwei@liuhongwei-virtual-machine:~/demo06_ws$ rostopic list 
/addint/cancel
/addint/feedback
/addint/goal
/addint/result
/addint/status
/rosout
/rosout_agg

④测试1：/addint/goal

liuhongwei@liuhongwei-virtual-machine:~/demo06_ws$ rostopic pub /addint/goal demo01_action/addintsActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  num: 100" 

客户端有反应

[ INFO] [1650876798.563153907]: 客户端提交的目标值是：100

⑤测试2：/addint/feedback

⑤测试3：/addint/result

4.客户端实现 

Ⅰ.代码实现

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "demo01_action/addintsAction.h"

//响应成功时回调函数
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,"");
    ros::init(argc,argv,"addints_void (const demo01_action::addintsFeedbackConstPtr &feedback)lient");
    ros::NodeHandle nh;

    //创建 action 客户端对象
    actionlib::SimpleActionClient client(nh,"addint");

    //发送请求
    //1.等待服务
    ROS_INFO("等待服务器启动");
    client.waitForServer();
    //2.
    /*
    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 = 100;  
    client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
    //spin
    ros::spin();

    return 0;
}

Ⅱ.修改配置文件并运行

7.3 动态参数 

7.3.1 引言

1.场景

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

机器人调试时，需要修改机器人轮廓信息(长宽高)、传感器位姿信息....，如果这些信息存储在参数服务器中，那么意味着需要重启节点，才能使更新设置生效，但是希望修改完毕之后，某些节点能够即时更新这些参数信息。

在ROS中针对这种场景已经给出的解决方案: dynamic reconfigure 动态配置参数。

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

机器人标定线速度角速度时，轮胎半径和轮间距每个车不一定，需要调试这两个数据。

如果放到参数服务器，则修改后节点要重新启动，而修改又是比较频繁的...... 

2.概念

一种可以在运行时更新参数而无需重启节点的参数配置策略。 

3.作用

主要应用于需要动态更新参数的场景，比如参数调试、功能切换等。典型应用:导航时参数的动态调试。

 7.3.2 动态参数客户端

1.需求

编写两个节点，一个节点可以动态修改参数，另一个节点时时解析修改后的数据。

2.流程

• 新建并编辑 .cfg 文件;
• 编辑CMakeLists.txt;
• 编译。

3.具体实现

Ⅰ.新建功能包，添加依赖:roscpp rospy std_msgs dynamic_reconfigure 

Ⅱ.添加.cfg文件

新建 cfg 文件夹，添加 xxx.cfg 文件(并添加可执行权限)，cfg 文件其实就是一个 python 文件,用于生成参数修改的客户端(GUI)

Ⅲ.修改配置文件

## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/dr.cfg
 )

Ⅳ.code

#! /usr/bin/env python
#encoding:utf-8

"""
动态参数客户端：
1.导包
2.创建一个参数生成器
3.向参数生成器中添加参数
4.配置节点并退出
"""

from dynamic_reconfigure.parameter_generator_catkin import *

#创建一个参数生成器
gen = ParameterGenerator()

#向参数生成器中添加参数
#参数名字 类型 --- 描述 默认值 最小值 最大值 
gen.add("int_param",int_t,0,"整数参数",10,1,100)

#配置节点并退出
#功能包名 节点名称 名字
exit(gen.generate("demo02_dr","dr_client","dr"))

 7.3.3 动态参数服务端

1.流程

新建并编辑 c++ 文件;
编辑CMakeLists.txt;
编译并执行。

2.code

#include "ros/ros.h"
#include "dynamic_reconfigure/server.h"
#include "demo02_dr/drConfig.h"
/*
    动态参数服务端：
    1.包含相关头文件
    2.初始化ros节点
    3.创建服务端对象
    4.回调函数解析修改后的参数
    5.回旋spin
*/

//解析动态参数
void cb(demo02_dr::drConfig &config, uint32_t level)
{
    ROS_INFO("修改后的整形数据是：%d",config.int_param);
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dr_server");

    //3.创建服务端对象
    dynamic_reconfigure::Server server;

    //4.回调函数解析修改后的参数
    //void setCallback(const boost::function &callback)
    server.setCallback(boost::bind(&cb,_1,_2));

    //5.回旋spin
    ros::spin();
    return 0;
}

3.测试

Ⅰ.启动服务端

liuhongwei@liuhongwei-virtual-machine:~/demo06_ws$ rosrun demo02_dr dr
dr01 
liuhongwei@liuhongwei-virtual-machine:~/demo06_ws$ rosrun demo02_dr dr01
[ INFO] [1650962746.440123961]: 修改后的整形数据是：10

 Ⅱ.启动客户端

启动rqt工具箱

4. 扩展：补齐数据类型

#! /usr/bin/env python
#encoding:utf-8

"""
动态参数客户端：
1.导包
2.创建一个参数生成器
3.向参数生成器中添加参数
4.配置节点并退出
"""

from dynamic_reconfigure.parameter_generator_catkin import *

#创建一个参数生成器
gen = ParameterGenerator()

#向参数生成器中添加参数
#参数名字 类型 --- 描述 默认值 最小值 最大值 
gen.add("int_param",int_t,0,"整数参数",10,1,100)
gen.add("double_param",double_t,0,"浮点参数",1.57,0,3.14)
gen.add("str_param",str_t,0,"字符窜参数","hello")
gen.add("bool_param",bool_t,0,"布尔参数",True)
myList = gen.enum([gen.const("small",int_t,0,"small car"),
                                        gen.const("normal",int_t,1,"normal car"),
                                        gen.const("big",int_t,2,"big car")
                                        ],"car_size")
gen.add("list_param",int_t,0,"列表参数",0,0,2,edit_method=myList)

#配置节点并退出
#功能包名 节点名称 名字
exit(gen.generate("demo02_dr","dr_client","dr"))

#include "ros/ros.h"
#include "dynamic_reconfigure/server.h"
#include "demo02_dr/drConfig.h"
/*
    动态参数服务端：
    1.包含相关头文件
    2.初始化ros节点
    3.创建服务端对象
    4.回调函数解析修改后的参数
    5.回旋spin
*/

//解析动态参数
void cb(demo02_dr::drConfig &config, uint32_t level)
{
    ROS_INFO("修改后的整形数据是：%d,%.2f,%s,%d,%d",config.int_param,config.double_param,config.str_param.c_str(),config.bool_param,config.list_param);
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dr_server");

    //3.创建服务端对象
    dynamic_reconfigure::Server server;

    //4.回调函数解析修改后的参数
    //void setCallback(const boost::function &callback)
    server.setCallback(boost::bind(&cb,_1,_2));

    //5.回旋spin
    ros::spin();
    return 0;
}

7.4 pluginlib

7.4.1 概念

1.简介

pluginlib直译是插件库，所谓插件字面意思就是可插拔的组件，比如:以计算机为例，可以通过USB接口自由插拔的键盘、鼠标、U盘...都可以看作是插件实现，其基本原理就是通过规范化的USB接口协议实现计算机与USB设备的自由组合。同理，在软件编程中，插件是一种遵循一定规范的应用程序接口编写出来的程序，插件程序依赖于某个应用程序，且应用程序可以与不同的插件程序自由组合。

2.场景

1.导航插件:在导航中，涉及到路径规划模块，路径规划算法有多种，也可以自实现，导航应用时，可能需要测试不同算法的优劣以选择更合适的实现，这种场景下，ROS中就是通过插件的方式来实现不同算法的灵活切换的。

2.rviz插件:在rviz中已经提供了丰富的功能实现，但是即便如此，特定场景下，开发者可能需要实现某些定制化功能并集成到rviz中，这一集成过程也是基于插件的。

3.概念

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

4.作用

• 结构清晰；

• 低耦合，易修改，可维护性强；

• 可移植性强，更具复用性；

• 结构容易调整，插件可以自由增减；

7.4.2 使用 

1.需求说明

以插件的方式实现正多边形的相关计算。

2.实现流程

1. 准备；

2. 创建基类；

3. 创建插件类；

4. 注册插件;

5. 构建插件库;

6. 使插件可用于ROS工具链;

   • 配置xml

   • 导出插件

7. 使用插件;

8. 执行。

3. 具体实现

Ⅰ.准备

新建功能包，导入依赖 roscpp pluginlib。

配置c_cpp_properties.json 

Ⅱ.创建基类

#ifndef DBX_BASE_H
#define DBX_BASE_H

namespace dbx_base_ns
{
    /*
    注意：必须保证基类中包含无参构造
    */
    class Dbx_Base
    {
        protected:
            Dbx_Base(){}

        public:
            //计算周长的函数
            virtual double getlength() = 0;
            //初始化边长的函数
            virtual void init(double side_length) = 0;
    };
};

#endif

Ⅲ.创建插件类

#ifndef XXX_POLYGON_PLUGINS_H_
#define XXX_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

Ⅳ.注册插件

在 src 目录下新建 polygon_plugins.cpp 文件，内容如下:

//pluginlib 宏，可以注册插件类
#include 
#include 
#include 

//参数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)

Ⅵ.使插件可用于ROS工具链

配置xml

功能包下新建文件:polygon_plugins.xml,内容如下:

  
  
    
    This is a triangle plugin.
  
  
    This is a square plugin.
  

导出插件

package.xml文件中设置内容如下:

标签的名称应与基类所属的功能包名称一致，plugin属性值为上一步中创建的xml文件。

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

Ⅶ.使用插件

src 下新建c++文件:polygon_loader.cpp，内容如下:

//类加载器相关的头文件
#include 
#include 

int main(int argc, char** argv)
{
  //类加载器 -- 参数1:基类功能包名称 参数2:基类全限定名称
  pluginlib::ClassLoader poly_loader("xxx", "polygon_base::RegularPolygon");

  try
  {
    //创建插件类实例 -- 参数:插件类全限定名称
    boost::shared_ptr triangle = poly_loader.createInstance("polygon_plugins::Triangle");
    triangle->initialize(10.0);

    boost::shared_ptr 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})

编译然后执行:polygon_loader，结果如下:

[ INFO] [WallTime: 1279658450.869089666]: Triangle area: 43.30
[ INFO] [WallTime: 1279658450.869138007]: Square area: 100.00

7.5 pluginlib 

7.5.1 应用场景

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

现在需要编写一个相机驱动，在该驱动中有两个节点实现:其中节点A负责发布原始图像数据，节点B订阅原始图像数据并在图像上标注人脸。如果节点A与节点B仍按照之前实现，两个节点分别对应不同的进程，在两个进程之间传递容量可观图像数据，可能就会出现延时的情况，那么该如何优化呢？

ROS中给出的解决方案是:Nodelet，通过Nodelet可以将多个节点集成进一个进程。

 1.概念

nodelet软件包旨在提供在同一进程中运行多个算法(节点)的方式，不同算法之间通过传递指向数据的指针来代替了数据本身的传输(类似于编程传值与传址的区别)，从而实现零成本的数据拷贝。

nodelet功能包的核心实现也是插件，是对插件的进一步封装:

• 不同算法被封装进插件类，可以像单独的节点一样运行；
• 在该功能包中提供插件类实现的基类:Nodelet；
• 并且提供了加载插件类的类加载器:NodeletLoader。

2. 作用

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

7.5.2 使用演示

在ROS中内置了nodelet案例，我们先以该案例演示nodelet的基本使用语法，基本流程如下:

1. 案例简介；
2. nodelet基本使用语法；
3. 内置案例调用。

1.案例 

在该案例中，定义了一个Nodelet插件类:Plus，这个节点可以订阅一个数字，并将订阅到的数字与参数服务器中的 value 参数相加后再发布。

需求:再同一线程中启动两个Plus节点A与B，向A发布一个数字，然后经A处理后，再发布并作为B的输入，最后打印B的输出。

2.基本使用语法

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.内置案例---命令行调用

Ⅰ.创建管理工具--经理大孙

liuhongwei@liuhongwei-virtual-machine:~$ rosrun nodelet nodelet manager __name:=dasun

liuhongwei@liuhongwei-virtual-machine:~$ rosnode list
/add_server
/addints
/dasun

Ⅱ.向dasun办公室内添加插件xiaowang

liuhongwei@liuhongwei-virtual-machine:~$ rosrun nodelet nodelet load nodelet_tutorial_math/Plus dasun __name:=xiaowang _value:=100

liuhongwei@liuhongwei-virtual-machine:~$ rosnode list 
/dasun
/xiaowang

liuhongwei@liuhongwei-virtual-machine:~$ rosparam list 
/xiaowang/value

Ⅲ.向dasun办公室内添加插件ergou

liuhongwei@liuhongwei-virtual-machine:~$ rosrun nodelet nodelet load nodelet_tutorial_math/Plus dasun __name:=ergou _value:=-50

liuhongwei@liuhongwei-virtual-machine:~$ rosnode list 
/dasun
/ergou
/xiaowang

liuhongwei@liuhongwei-virtual-machine:~$ rostopic list 
/dasun/bond
/ergou/in
/ergou/out
/xiaowang/in
/xiaowang/out

Ⅳ.话题重定向

liuhongwei@liuhongwei-virtual-machine:~$ rosrun nodelet nodelet load nodelet_tutorial_math/Plus dasun __name:=ergou _value:=-50 /ergou/in:=/xiaowang/out

liuhongwei@liuhongwei-virtual-machine:~$ rostopic list 
/dasun/bond
/ergou/out
/xiaowang/in
/xiaowang/out

发现ergou/in没了

Ⅴ.逻辑链

liuhongwei@liuhongwei-virtual-machine:~$ rostopic pub -r 10 /xiaowang/in std_msgs/Float64 "data: 10.0" 

向小王发送数据10

liuhongwei@liuhongwei-virtual-machine:~$ rostopic echo /xiaowang/out 
data: 110.0
---

xiaowang out为110

但此时输出ergou/out

^Cliuhongwei@liuhongwei-virtual-machine:~$ rostopic echo /ergou/out 
data: 60.0
---
data: 60.0

这时为什么？这个10给小王处理，小王处理完事之后（+100），ergou/in是110，-50以后就是60了

4. 内置案例---launch文件调用

启动launch节点，查看节点和话题

liuhongwei@liuhongwei-virtual-machine:~$ rosnode list 
/dasun
/ergou
/mymanager
/n1
/n2
/rosout
/rostopic_118891_1650978503995
/xiaowang
liuhongwei@liuhongwei-virtual-machine:~$ rostopic list 
/addint/cancel
/addint/feedback
/addint/goal
/addint/result
/addint/status
/dasun/bond
/ergou/out
/mymanager/bond
/n1/in
/n1/out
/n2/out
/rosout
/rosout_agg
/xiaowang/in
/xiaowang/out

测试结果与上面相同

4.nodelet实现基本流程

Ⅰ.介绍：

nodelet本质也是插件，实现流程与插件实现流程类似，并且更为简单，不需要自定义接口，也不需要使用类加载器加载插件类。

Ⅱ.需求

参考 nodelet 案例，编写 nodelet 插件类，可以订阅输入数据，设置参数，发布订阅数据与参数相加的结果。

Ⅲ.步骤

1. 准备；

2. 创建插件类并注册插件;

3. 构建插件库;

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

5. 执行。

5.实现

Ⅰ.准备

新建功能包，导入依赖: 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("out",100);
        sub = nh.subscribe("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)，内容如下：

    
        hello
    

导出插件

Ⅴ.执行

可以通过launch文件执行nodelet，示例内容如下:

运行launch文件，参考上面 p1发布数据，并订阅p2输出的数据，最终运行结果类似。