自定义nodelet插件
参考链接：
http://wiki.ros.org/nodelet
http://wiki.ros.org/nodelet/Tutorials/Running a nodelet
http://wiki.ros.org/nodelet/Tutorials/Porting nodes to nodelets
https://blog.****.net/zyh821351004/article/details/52143309
ROS的数据通信是以XML-RPC的方式,在graph结构中以topic,service和param的方式传输数据，天生的数据交互存在一定的延时和阻塞。Nodelet 包就是改善这一状况设计的， 使得多个算法运行在同一个过程中，并且算法间数据传输无需拷贝就可实现。 详见http://wiki.ros.org/nodelet。 简单的讲就是可以将以前启动的多个node捆绑在一起manager，使得同一个manager里面的topic的数据传输更快，数据通讯中roscpp采用boost shared pointer方式进行publish调用，实现zero copy。
以下为本人自己在学习相关资料，自己做的一个测试用例，供参考。
1 创建工作空间
makedir nodelet_ws/src
2 创建nodelet_test包
catkin_create_pkg nodelet_test roscpp std_msgs nodelet
3 创建nodelet_test/src/math_plus.cpp文件
math_plus.cpp
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
namespace math_test
{
/* code */
class math_plus: public nodelet::Nodelet
{
public:
math_plus()
{
}
virtual ~math_plus()
{
}
private:
virtual void onInit()
{
ros::NodeHandle& private_nh = getPrivateNodeHandle();
private_nh.getParam("value", value_);
plus_result_pub_ = private_nh.advertise<std_msgs::Float64>("out", 10);
plus_param_sub_ = private_nh.subscribe("in", 10, &math_plus::plus_process_callback, this);
}
void plus_process_callback(const std_msgs::Float64::ConstPtr& input)
{
std_msgs::Float64Ptr output(new std_msgs::Float64());
output->data = input->data + value_;
NODELET_DEBUG("Adding %f to get %f", value_, output->data);
plus_result_pub_.publish(output);
}
private:
ros::Publisher plus_result_pub_;
ros::Subscriber plus_param_sub_;
double value_;
};
}
PLUGINLIB_EXPORT_CLASS(math_test::math_plus, nodelet::Nodelet);
math_plus.cpp解析：
（1）通过 plus_param_sub_ = private_nh.subscribe(“in”, 10, &math_plus::plus_process_callback, this)订阅名为math_plus/in topic的数据
（2）与通过参数服务器（private_nh.getParam(“value”, value_)）获取到的参数value_进行相加
（3）将相加后的结果通过plus_result_pub_.publish(output)以topic的形式发布出去
插件注册：
PLUGINLIB_EXPORT_CLASS(math_test::math_plus, nodelet::Nodelet);
4 math_plus_plugin.xml
<library path="lib/libmath_plus">
<class name="nodelet_test/math_plus" type="math_test::math_plus" base_class_type="nodelet::Nodelet">
<description>
A node to add a value and republish.
</description>
</class>
</library>
5 CMakeList.txt
cmake_minimum_required(VERSION 2.8.3)
project(nodelet_test)
find_package(catkin REQUIRED COMPONENTS
nodelet
roscpp
std_msgs
)
catkin_package(
)
include_directories(
include ${catkin_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
add_library(math_plus
src/math_plus.cpp
)
add_dependencies(math_plus ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(math_plus
${catkin_LIBRARIES}
)
6 package.xml
<?xml version="1.0"?>
<package format="2">
<name>nodelet_test</name>
<version>0.0.0</version>
<description>The nodelet_test package</description>
<maintainer email="[email protected]">zyn</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>nodelet</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>nodelet</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<nodelet plugin="${prefix}/math_plus_plugin.xml"/>
</export>
</package>
7 launch文件
math_plus_launch.launch
<launch>
<node pkg="nodelet" type="nodelet" name="math_plus_manage" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="math_plus" args="load nodelet_test/math_plus math_plus_manage" output="screen">
<param name="value" type="double" value="10"/>
</node>
</launch>
步骤 3 4 5 6 7之间的联系：
（1）步骤4 中的libmath_plus 由步骤5 中的add_library(math_plus src/math_plus.cpp)生成
（2）步骤4 中 的 type 和 base_class_type 由步骤3中PLUGINLIB_EXPORT_CLASS(math_test::math_plus, nodelet::Nodelet)确定
（3）步骤6 中 的math_plus_plugin.xml 为步骤 4 中创建的文件
（4）步骤7 中 的nodelet_test/math_plus 与步骤4 中的 class name=“nodelet_test/math_plus” 相对应
（5）步骤7 中的 为步骤3 中的 value参数提供数据，数据值为double型数值10
math_plus_launch.launch文件:
（1）启动两个notelet节点，但参数不同分别为args="manager"和args=“load nodelet_test/math_plus math_plus_manage”，可以浅显的认为参数manager对应的节点充当管理层，另一个或多个节点充当被管理者
（2）参数args=“load nodelet_test/math_plus math_plus_manage” 中，
load ：加载
nodelet_test/math_plus：插件类名称
math_plus_manage：插件所属的管理层名称
nodelet可以同时创建多个管理层，可以根据需求将nodelet分配到对应的manage层中，如
example.launch
<launch>
<node pkg="nodelet" type="nodelet" name="math_plus_manage" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="math_plus" args="load nodelet_test/math_plus math_plus_manage" output="screen">
<param name="value" type="double" value="10"/>
</node>
<node pkg="nodelet" type="nodelet" name="math_plus1" args="load nodelet_test/math_plus math_plus_manage" output="screen">
</node>
<node pkg="nodelet" type="nodelet" name="math_plus2" args="load nodelet_test/math_plus math_plus_manage" output="screen">
</node>
<node pkg="nodelet" type="nodelet" name="math_plus3" args="load nodelet_test/math_plus math_plus_manage" output="screen">
</node>
<node pkg="nodelet" type="nodelet" name="math_plus_manage_2" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="math_plus4" args="load nodelet_test/math_plus math_plus_manage_2" output="screen">
</node>
<node pkg="nodelet" type="nodelet" name="math_plus5" args="load nodelet_test/math_plus math_plus_manage_2" output="screen">
</node>
<node pkg="nodelet" type="nodelet" name="math_plus6" args="standalone nodelet_test/math_plus" output="screen">
</node>
</launch>
example.launch中表明，math_plus、math_plus1、math_plus2、math_plus3对应的nodelet所属manager为math_plus_manage，math_plus4、math_plus5所属manager为math_plus_manage_2，test6独立存在，他们之间的数据交互按manager进行划分，示意如下：
8 运行测试
（1）启动nodelet
roslaunch nodelet_test math_plus_launch.launch
（2）发布/math_plus/in topic 数值为 1
rostopic pub -r 1 /math_plus/in std_msgs/Float64 1
（3）rostopic echo /math_plus/out查看输出数据
[email protected]:~/zyn_test/nodelet_ws$ rostopic echo /math_plus/out
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
data: 11.0
---
launch文件中的参数值为10， 发布的topic数据为1 ，相加后的结果为 11