参考:nodelet的使用方法以及传输时间测试 ;ROS中pluginlib的使用总结;ROS nodelet初探;ROS与C++入门教程-nodelet-Node封装为Nodelet
Nodelet:
创建一个nodelet_manager进程,将一个或多个nodelet结点加入其中。每一个nodelet可看作一个node(有细微区别)。由于node之间存在多进程通信问题,所以nodelet中将多个结点跑到一个进程下,其中每个nodelet为一个轻量级线程。进程内部内存共享,实现0拷贝。
1.定义两个类,Pub和Sub
#ifndef _Nodelet_Pub_H_
#define _Nodelet_Pub_H_
#include <ros/ros.h>
#include <iostream>
#include <msgs/data.h>
#include <nodelet/nodelet.h>
#include <pthread.h>
#include <pluginlib/class_list_macros.h>
using namespace std;
namespace Pub_nodelet
{
class Pub : public nodelet::Nodelet
{
public:
Pub(){}
~Pub() {
pthread_join(tid, NULL);
}
static void* PubMainLoop(void* tmp)
{
Pub* pub = (Pub*) tmp;
int num = 1;
msgs::data test_msg;
for (int i = 0; i < 100000; i++) {
test_msg.a.push_back(i);
}
ros::Rate loop_rate(1);
while (ros::ok()) {
ROS_INFO("%d,publish!", num++);
pub->pub_a.publish(test_msg);
ros::spinOnce();
loop_rate.sleep();
}
}
private:
pthread_t tid;
ros::Publisher pub_a;
virtual void onInit()
{
ros::NodeHandle nh = getNodeHandle();
this->pub_a = nh.advertise<msgs::data>("int_a", 1);
pthread_create(&tid, NULL, Pub::PubMainLoop, (void*)this);
}
};
} //namespace Pub_nodelet
#endif
类继承nodelet::Nodelet基类;
onInit函数相当于main函数,“当 nodelet 插件类被 nodelet_manager 加载时,nodelet 插件类的 onInit 方法就会被调用,用于初始化插件类。 onInit 方法不能被阻塞,只用于初始化,如果 nodelet 插件需要执行循环任务,要将其放入线程中执行。"若没有加入新线程,会出现nodelet开启失败问题。
#include "nodelet_pub.h"
///注册插件类 写在命名空间外面
PLUGINLIB_EXPORT_CLASS(Pub_nodelet::Pub, nodelet::Nodelet)
利用 PLUGINLIB_EXPORT_CLASS 宏来注册插件类,必须包含#include <pluginlib/class_list_macros.h>头文件。
2.编写plugins.xml文件,其中包含了library中的每个类和它继承的类。
具体写法:ROS/pluginlib
<!--pub_plugins.xml-->
<library path="lib/libpubPlugins">
<class name="nodelet_pub" type="Pub_nodelet::Pub" base_class_type="nodelet::Nodelet">
<description>
This is the Pub nodelet.
</description>
</class>
</library>
<!--sub_plugins.xml-->
<library path="lib/libsubPlugins">
<class name="nodelet_sub" type="Sub_nodelet::Sub" base_class_type="nodelet::Nodelet">
<description>
This is Sub nodelet.
</description>
</class>
</library>
其中,path中libpubPlugins为devel/lib文件夹下生成的动态链接库名,库名为lib+add_library<name>.so.
class中type=类所在命名空间::类名,base_class_type=type中类继承的基类,name为对类重命名(可不写)。
3.在cMakeList.txt中加入
catkin_package(
CATKIN_DEPENDS roscpp std_msgs nodelet ##add nodelet
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
add_library(pubPlugins src/pub.cpp)
add_library(subPlugins src/sub.cpp) #${PROJECT_NAME}
target_link_libraries(pubPlugins ${catkin_LIBRARIES})
target_link_libraries(subPlugins ${catkin_LIBRARIES})
add_library数目决定生成的.so库数目。
4.package.xml中加入
<build_depend>nodelet</build_depend> <!-- add -->
<exec_depend>nodelet</exec_depend> <!-- add -->
<export>
<nodelet plugin="${prefix}/plugins/pub_plugins.xml" />
<nodelet plugin="${prefix}/plugins/sub_plugins.xml" />
</export>
plugins为xml所在文件夹名称
5.运行
1)终端下依次打开
编译后,开四个终端,首先
source devel/setup.bash
然后分别输入
roscore
rosrun nodelet nodelet manager __name:=manager_nodelet
rosrun nodelet nodelet load nodelet_sub manager_nodelet _output:=screen
rosrun nodelet nodelet load nodelet_pub manager_nodelet _output:=screen
其中,nodelet_sub和nodelet_pub为在plugins.xml文件中对class的重命名。
2)使用launch文件打开
<launch>
<node pkg="nodelet" type="nodelet" name="manager_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="Pub" args="load nodelet_pub manager_nodelet" output="screen"/>
<node pkg="nodelet" type="nodelet" name="Sub" args="load nodelet_sub manager_nodelet" output="screen"/>
</launch>
其中走的弯路,等有空再写吧。。