ROS下的nodelet
参考来源：http://wiki.ros.org/nodelet
http://wiki.ros.org/nodelet/Tutorials/Running%20a%20nodelet
http://wiki.ros.org/nodelet/Tutorials/Porting%20nodes%20to%20nodelets
http://wiki.ros.org/nodelet_topic_tools?distro=indigo
一、nodelet包总结。
nodelet包提供一种在一个进程中同时运行多个算法，并且算法间消息实现零拷贝传输。这个包提供了实现一个nodelet所需的nodelet基类,以及用于实例化nodelets的NodeletLoader类。roscpp已经优化了在同一节点上发布和订阅调用函数的零拷贝指针传输。为此nodelets允许动态加载类到相同的节点, 他们提供nodelet简单的单独的命名空间使nodelet即便是在同一进程也像一个独立的节点。在运行时动态的载入使用到了pluginlib。由nodelets组成的高吞吐量数据流加载到同一个进程避免了大量的数据复制和网络传输。
1.设计目标：
（1）利用现有的c++ ros 接口。
（2）允许nodelets间数据的零拷贝传输。
（3）像插件一样动态加载，打破对编译时间的依赖。
（4）性能改进的同时，位置透明。
（5）使nodelet和node代码编写只有细微的差别。
2.技术：
（1）定义所有nodelet继承的动态加载的基类nodelet::Nodelet，使用pluginlib动态加 载nodelet。
（2）提供一个命名空间，自动的重绘参数和变量。
（3）可以向nodelet_manager载入多个nodelet，这些nodelets间通过roscpp提供的共 享指针进行数据间的零拷贝传输。
3.基本的使用指令：
（1）nodelet load pkg/Type manager ：向manager载入pkg/Type类型的nodelet。
（2）nodelet standalone pkg/Type：以独立的节点形式载入pkg/Type类型的nodelet。
（3）nodelet unload name manager：从manager卸载name节点。
（4）nodelet manager：载入nodelet管理器节点。
4.API:
(1)Nodelet Base Class:nodelet::Nodelet
Public methods:
Nodelet()//动态载入时的构造函数
void init (const std::string& name, const ros::M_string& remapping_args, const std::vector<std::string>& my_argv);
//nodelet初始化函数，参数从manager加载，用于初始化nodelet基类，随后调用onInit()函数。
子类中使用的Protected members and methods:
std::string getName()//获取nodelet名称
ros::NodeHandle& getNodeHandle ()//获取节点句柄
ros::NodeHandle& getPrivateNodeHandle ()//获取私有节点句柄
ros::NodeHandle& getMTNodeHandle () //获取具有多线程回调队列的节点句柄
ros::NodeHandle& getMTPrivateNodeHandle ()//获取具有多线程回调队列的私有节点句柄
ros::CallbackQueue& getMTCallbackQueue () //获取多线程回调函数队列
std::vector<std::string> getMyArgv()//获取命令行参数
子类中初始化ROS API的方法：
virtual void onInit () = 0 //虚函数，必须在子类中重载。所有ros接口的初始化必须放在这个函数中。
（2）NODELET ROSCONSOLE MACROS：DEBUG, INFO, WARN, ERROR, and FATAL.信息显示等级。
（3）必须采用共享指针shared_ptr发布信息，以便实现零拷贝发送和订阅。相关内容参考roscpp/Overview/Publishers and Subscribers#Intraprocess_Publishing
5.线程模式：
一个nodelet manager拥有一个线程池，所有的nodelets共享该线程池。参数num_worker_threads设定线程池中线程的数量。Nodelet有两种线程API，默认的是对于所有的回调函数采用单线程模式，另一种是多线程模式。在一个manager中必须设置足够的线程数，避免回调函数发生时线程池中没有空闲的线程。
（1）onInit这个方法在初始化中调用，不应当被阻塞或执行繁重的工作。
（2）Single Threaded API：
使用getNodeHandle()和getPrivateNodeHandle()方法将导致所有的回调函数连续到达。
（3）Multi Threaded API：使用getMTNodeHandle()和getMTPrivateNodeHandle()方法将使回调函数被分散在整个nodelet manager的线程池中。
（4）Additional Threads：nodelet可以创建自己的线程，但必须在析构函数中清理。
二、运行一个nodelet。
1.运行一个nodelet manager,它监控ros服务，可以载入nodelet节点。
rosrun nodelet nodelet manager __name:=nodelet_manager
2.载入nodelets:
rosrun nodelet nodelet load nodelet_tutorial_math/Plus nodelet_manager __name:=nodelet1 nodelet1/in:=foo _value:=1.1
使用nodelet包下的nodelet可执行程序载入nodelet节点-nodelet_tutorial_math/Plus到nodelet_manager节点主机中，nodelet的名称为nodelet1，重绘话题foo为节点需要的话题nodelet1/in，设置节点参数=1.1.
3.测试:
rostopic pub /foo std_msgs/Float64 5.0 -r 10 //用/foo发布std_msgs/Float类型的消息。
rostopic echo /nodelet1/out //查看输出
4.使用roslaunch文件来安装；
例：
<launch>
//安装名称为standalone_nodelet的nodelet manager。
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
//载入名为Plus的nodelet到standalone_nodelet manager中，
//重绘话题：/Plus/out=remapped_output
<node pkg="nodelet" type="nodelet" name="Plus"
args="load nodelet_tutorial_math/Plus standalone_nodelet">
<remap from="/Plus/out" to="remapped_output"/>
</node>
//使用文件设置参数。
<rosparam param="Plus2" file="$(find nodelet_tutorial_math)/plus_default.yaml"/>
//继续载入第二个nodelet，并使用文件设置参数。
<node pkg="nodelet" type="nodelet" name="Plus2" args="load nodelet_tutorial_math/Plus standalone_nodelet">
<rosparam file="$(find nodelet_tutorial_math)/plus_default.yaml"/>
</node>
//第三个nodelet。
<node pkg="nodelet" type="nodelet" name="Plus3" args="standalone nodelet_tutorial_math/Plus">
<param name="value" type="double" value="2.5"/>
<remap from="Plus3/in" to="Plus2/out"/>
</node>
</launch>
三、移植node为nodelet。
1.流程：
（1）添加必要的 #includes。
（2）移除主函数int main()。
（3）创建nodelet::Nodelet子类。
（4）把构造函数移植到onInit()。
（5）添加PLUGINLIB_EXPORT_CLASS宏定义。
（6）在package.xml中添加nodelet编译、运行依赖项nodelet。
（7）将nodelet加入到package.xml的<export> 部分。
（8）定义.xml文件使nodelet成为一个插件。
（9）在CMakeLists.txt中添加rosbuild_add_executable, add a rosbuild_add_library。
2、最小的nodelet：
（1）MyNodeletClass.h
#include <nodelet/nodelet.h>
namespace example_pkg
{
class MyNodeletClass : public nodelet::Nodelet
{
public:
virtual void onInit();
};
}
（2）MyNodeletClass.cpp
// this should really be in the implementation (.cpp file)
#include <pluginlib/class_list_macros.h>
// watch the capitalization carefully
PLUGINLIB_EXPORT_CLASS(example_pkg::MyNodeletClass, nodelet::Nodelet)
namespace example_pkg
{
void MyNodeletClass::onInit()
{
NODELET_DEBUG("Initializing nodelet...");
}
}
（3）nodelet_plugins.xml
<library path="lib/libMyNodeletClass">
<class name="example_pkg/MyNodeletClass" type="example_pkg::MyNodeletClass" base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
</library>
（4）package.xml
...
<build_depend>nodelet</build_depend>
<run_depend>nodelet</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
...
（5）mynodelet.launch
<launch>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="MyNodeletClass" args="load example_pkg/MyNodeletClass standalone_nodelet" output="screen">
</node>
</launch>
四、nodelet_topic_tools
1.NodeletMUX：多路复用器。一个nodelet将N（<8）个输入话题发布到一个输出话题上。
（1）例： <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
<!-- MUX the data onto one topic -->
<node pkg="nodelet" type="nodelet" name="data_mux" args="load pcl/NodeletMUX pcl_manager" output="screen">
<rosparam>
input_topics: [/passthrough/output, /normal_estimation/output]//将多个话题作为输入话题。
</rosparam>
</node>
接收/passthrough/output、/normal_estimation/output话题上的数据，发布到/data_mux/output话题上。
（2）NodeletMUX在库文件中的编译要采用类似以下的形式：
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, message_filters::Subscriber<sensor_msgs::PointCloud2> > NodeletMUX;
PLUGINLIB_DECLARE_CLASS (pcl, NodeletMUX, NodeletMUX, nodelet::Nodelet);
根据需要替换sensor_msgs::PointCloud2消息类型。
2.NodeletDEMUX：解多路复用器。一个nodelet接收一个话题，发布N(<8)个话题。
（1）例：
<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
<!-- MUX the data onto one topic -->
<node pkg="nodelet" type="nodelet" name="data_demux" args="load pcl/NodeletDEMUX pcl_manager" output="screen">
<rosparam>
output_topics: [/output1, /output2] //输出多个话题。
</rosparam>
</node>
从/data_demux/input上接收数据，发布到/data_demux/output1、/data_demux/output2。
（2）NodeletDEMUX在库文件中的编译要采用类似以下的形式：
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX, NodeletDEMUX, nodelet::Nodelet);
根据需要替换sensor_msgs::PointCloud2消息类型。
3.NodeletThrottle：在nodelet_topic_tools命名空间下。可以设定话题的发布速率。
（1）例：
<node pkg="nodelet" type="nodelet" name="nodelt_manager" args="manager" output="screen" />
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load my_pkg/NodeletThrottleImage nodelet_manager" output="screen">
<remap from="topic_in" to="camera/rgb/image_color"/>
<remap from="topic_out" to="camera/rgb/image_color_throttle"/>
</node>
（2）为了在你的库中编译NodeletThrottle，你需要添加类似以下：
#include <nodelet_topic_tools/nodelet_throttle.h>
#include <sensor_msgs/Image.h>
#include <pluginlib/class_list_macros.h>
typedef nodelet_topic_tools::NodeletThrottle<sensor_msgs::Image> NodeletThrottleImage;
PLUGINLIB_DECLARE_CLASS (my_pkg, NodeletThrottleImage, NodeletThrottleImage, nodelet::Nodelet);
根据需要替换sensor_msgs::Image消息类型。
五、移植node到nodelet的另一种方式，实例：
移植node为nodelet的实例：订阅来自Kinect的彩色图像，直接发布它。
1.node的c++程序。
（1）主程序。
int main (int argc, char** argv)
{
ros::init(argc, argv,"node_name");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
package_name::node_class class_object(node,priv_nh);
ros::spin();
return 0;
}
（2）节点类头文件。
//node_class.h
namespace package_name
{
class node_class
{
public:
node_class(ros::NodeHandle node,ros::NodeHandle private_nh);
node_class(){};
void image_callback(const sensor_msgs::ImageConstPtr& msg);
private:
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
}
}
(2)定义节点类。
// node_class.cpp
namespace package_name
{
node_class::node_class (ros::NodeHandle ndoe, ros::NodeHandle private_nh)
{
image_transport::ImageTransport it_(node);
image_sub_ = it_.subscribe("/camera/rgb/image_rect_color",1,&node_class::image_callback,this) ;
} //constructor
// define the image_callback, for now I just display the input image as it is, you can do all the modifications to the input image here and then publish it.
void node_class::image_callback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::imshow("in image", cv_ptr->image);
cv::waitKey(3);
image_pub_.publish(cv_ptr->toImageMsg());
} // image_callback
} // namespace
2.nodelet类。
（1）头文件
//nodelet_class.h
#include <nodelet/nodelet.h>
#include "node_class.h" //引用了节点类
namespace package_name
{
class nodelet_class: public nodelet::Nodelet
{
public:
nodelet_class(){}
~nodelet_class(){}
virtual void onInit();
boost::shared_ptr<node_class> inst_;//定义共享指针
};
}
（2）定义nodelet类
//nodelet_class.cpp
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include "nodelet_class.h"
namespace package_name
{
void nodelet_class::onInit()
{
NODELET_DEBUG("Initializing nodelet");
inst_.reset(new node_class(getNodeHandle(), getPrivateNodeHandle()));
}
}
PLUGINLIB_DECLARE_CLASS(package_name,nodelet_class, package_name :nodelet_class, nodelet::Nodelet)
（3）在package.xml中添加：
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
（4）nodelet_plugins.xml
<library path="lib/libnodelet_class">
<class name="package_name/nodelet_class" type="package_name::nodelet_class" base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
</library>
（5）launch_nodelet.launch
<launch>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="nodelet_class" args="load package_name/ nodelet_class standalone_nodelet" output="screen">
</node>
</launch>
##Note: Make sure you add a library for the nodelet_class which will be loaded while launching the nodelet in CMakeLists.txt
rosbuild_add_library(nodelet_class #add here the paths to all your source files)