阅读学习代码
pluginlib是一个C++库，可以实现为一个ROS包动态的加载和卸载插件。这里的插件通常是一些功能类，且以运行时可动态加载的库（如共享对象，动态链接库）的形式存在。借助pluginlib的帮助，用户不必关心自己的应用程序该如何链接包含自己想使用的的class的库（如定义class的头文件在哪里，如何定义的），因为pluginlib会在你调用时自动打开你需要的插件库（Note：需要提前将插件库注册到pluginlib）。使用插件来扩展或者修改应用程序的功能非常方便，不用改动源码重新编译应用程序，通过插件的动态加载即可完成功能的扩展和修改。
pluginlib利用了C++多态的特性，不同的插件只要使用统一的接口，便可以替换使用。这样用户通过调用在插件中实现的统一的接口函数，不需要更改程序，也不需要重新编译，更换插件即可实现功能修正。
利用pluginlib编写插件的方法大致包括如下四步：
原文链接：pluginlib
参考博客： pluginlib示例
1 ROS的数据通信在graph结构中以topic,service和param的方式传输数据，天生的数据交互存在一定的延时和阻塞。Nodelet 包就是改善这一状况设计的， 使得多个算法运行在同一个过程中，并且算法间数据传输无需拷贝就可实现。 详见http://wiki.ros.org/nodelet。 简单的讲就是可以将以前启动的多个node捆绑在一起manager，使得同一个manager里面的topic的数据传输更快。即同一个manager过程的nodelet数据传输zero copy 。
2 nodelet的manager可以加载多个的nodelets，nodelets间数据传输zero copy，有效避免数据copy和网络传输代价
3 支持pulgin的方式动态加载，基类nodelet::Nodelet， 任何nodelet继承自它可以使用plugin的方式动态加载。
4 Nodelets旨在提供一种在单机器单进程运行多个算法而不会在进程中传递消息时产生复制成本的方法。roscpp具有在同一节点内的发布和订阅调用之间进行零拷贝指针传递的优化,因此实际相当于不同nodelet在统一node运行,但写起来仍是多个node的写法.
参考博客：nodelet使用、nodelet原理
nodelet和node很大程度上保持一致, 对外来说, nodelet几乎可以视为node。
1、创建package：代码中package名字为msckf_vio
2、在include, src中创建一个类, 该类继承nodelet::Nodelet, 最好在类的外面加一个namespace, 可以以package名字做namespace. 类中有一个onInit()函数, 相当于main()函数.
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <msckf_vio/msckf_vio.h>
namespace msckf_vio {
class MsckfVioNodelet : public nodelet::Nodelet {
public:
MsckfVioNodelet() { return; }
~MsckfVioNodelet() { return; }
private:
virtual void onInit();
MsckfVioPtr msckf_vio_ptr;
};
} // end namespace msckf_vio
3、在cpp中实现onInit()函数, 编写自己的代码. nodelet允许将类动态加载到同一个节点，进而实现零拷贝通信，并且nodelet提供了简单的单独命名空间，使得尽管nodelet在同一个进程中，它使用时仍然像一个独立的节点，因此，它在运行时使用pluginlib是动态可加载的。因此，cpp中还需要加一行，导出为插件。
PLUGINLIB_EXPORT_CLASS(example_pkg::SampleNodelet, nodelet::Nodelet)
#include <msckf_vio/msckf_vio_nodelet.h>
namespace msckf_vio {
void MsckfVioNodelet::onInit() {
msckf_vio_ptr.reset(new MsckfVio(getPrivateNodeHandle()));
if (!msckf_vio_ptr->initialize()) {
ROS_ERROR("Cannot initialize MSCKF VIO...");
return;
}
return;
}
PLUGINLIB_EXPORT_CLASS(msckf_vio::MsckfVioNodelet,
nodelet::Nodelet);
} // end namespace msckf_vio
# Msckf Vio
add_library(msckf_vio
src/msckf_vio.cpp
src/utils.cpp
)
add_dependencies(msckf_vio
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(msckf_vio
${catkin_LIBRARIES}
${SUITESPARSE_LIBRARIES}
)
# Msckf Vio nodelet
add_library(msckf_vio_nodelet
src/msckf_vio_nodelet.cpp
)
add_dependencies(msckf_vio_nodelet
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(msckf_vio_nodelet
msckf_vio
${catkin_LIBRARIES}
)
<library path="lib/libmsckf_vio_nodelet">
<class name="msckf_vio/MsckfVioNodelet"
type="msckf_vio::MsckfVioNodelet"
base_class_type="nodelet::Nodelet">
<description>
Multi-State contraint Kalman filter for vision-
aided inertial navigation with observability constain.
</description>
</class>
</library>
<library path="lib/libimage_processor_nodelet">
<class name="msckf_vio/ImageProcessorNodelet"
type="msckf_vio::ImageProcessorNodelet"
base_class_type="nodelet::Nodelet">
<description>
Detect and track features in image sequence.
</description>
</class>
</library>
6、注册插件到ROS系统：package.xml中增加export项, 把上一个xml文件的位置配置进去.
<export>
<nodelet plugin="${prefix}/nodelets.xml"/>
</export>
注意：如果插件类与基类不在同一package，为了使插件的export生效，还必须添加对插件基类所在package的依赖。
<depend>nodelet</depend>
有的是：
<build_depend>my_plugin_test</build_depend>
<run_depend>my_plugin_test</run_depend>
7、在catkin_make执行成功之后，source develop/setup.bash，然后运行如下命令如果能正确看到输出polygon_plugin.xml则ok。
rospack plugins --attrib=plugin nodelet
正常的插件就可以使用了，在节点cpp里面包含头文件创建指针就好了，我们这里使用nodelet，用起来很方便
8、以launch文件运行，编写launch文件, 如下. 该launch打开两个nodelet, 更改第二个nodelet的name和args.
虽然说nodelet是为了方便消息传输，但其实这个代码中只包含了一个nodelet，没发挥这个快速传输的效果，调用就相当与node的调用，没什么区别，如果是多个nodelet就可以快速通信了。类似这样
<launch>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="SampleNodelet" args="load example_pkg/SampleNodelet standalone_nodelet" output="screen">
<node pkg="nodelet" type="nodelet" name="SampleNodelet2" args="load example_pkg/SampleNodelet2 standalone_nodelet" output="screen">
</node>
</launch>
解释一下：
第一行：创建mananger ，名字为standalone_nodelet
第二行：将example_pkg/SampleNodelet加入到manager standalone_nodelet
第三行：将另一个nodelet加入manager，他们就是一个进程了，共享内存，这两个nodelet消息传输很快
命令行调用:
#启动nodelet管理器
roscore
rosrun nodelet nodelet manager __name:=nodelet_manager
#启动自己的nodelet
rosrun nodelet nodelet load nodelet_tutorial_math/Plus nodelet_manager __name:=nodelet1 nodelet1/in:=foo _value:=1.1
<!-- Msckf Vio Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="vio"
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="publish_tf" value="true"/>
........
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
<remap from="~imu" to="/imu0"/>
<remap from="~features" to="image_processor/features"/>
</node>
</group>
launch常用配置
<launch> <!--根标签-->
<node> <!--需要启动的node及其参数-->
<include> <!--包含其他launch-->
<machine> <!--指定运行的机器-->
<env-loader> <!--设置环境变量-->
<param> <!--定义参数到参数服务器-->
<rosparam> <!--加载yaml文件中的参数到参数服务器-->
<arg> <!--定义变量-->
<remap> <!--设定 topic 映射-->
<group> <!--设定分组-->
</launch> <!--根标签-->
参考: launch使用
启动文件的核心是启动ROS节点，采用标签定义，语法如下：
<node name="node-name" pkg="package-name" type="executable-name"/>
从上边的定义规则可以看出，在启动文件中启动一个节点需要三个属性：name、pkg和type。其中name属性用来定义节点运行的名称，将覆盖节点中ros::init()定义的节点名称；pkg属性定义节点所在的功能包名称，type属性定义节点的可执行文件名称，这两个属性等同于在终端中使用rosrun命令执行节点时的输入参数。这是三个最常用的属性，在某些情况下，我们还有可能用到以下属性：
属性属性作用output="screen"终端输出转储在当前的控制台上，而不是在日志文件中respawn="true"当roslaunch启动完所有该启动的节点之后，会监测每一个节点，保证它们正常的运行状态。对于任意节点，当它终止时，roslaunch 会将该节点重启required="true"当被此属性标记的节点终止时，roslaunch会将其他的节点一并终止。注意此属性不可以与respawn="true"一起描述同一个节点ns = "NAME_SPACE"这个属性可以让你在自定义的命名空间里运行节点args = "arguments"节点需要的输入参数
二、参数设置
为了方便设置和修改，launch文件支持参数设置的功能，类似于编程语言中的变量声明。关于参数设置的标签元素有两个：、，一个代表parameter，另一个代表argument。这两个标签元素翻译成中文都是“参数”的意思，但是这两个“参数”的意义是完全不同的。
2.1 param
parameter是ROS系统运行中的参数，存储在参数服务器中。在launch文件中可以通过param元素加载parameter。launch文件执行后，parameter就加载到ROS的参数服务器上了。
比如现在在参数服务器中添加一个名为demo_param，值为666的参数
<param name="demo_param" type="int" value="666"/>
运行launch文件后，demo_param这个parameter的值就设置为666，并且加载到ROS参数服务器上了。但是在很多复杂的系统中，参数的数量很多，如果这样一个一个的设置会非常麻烦，ROS也为我们提供了另外一种类似的参数加载方式——rosparam：
<rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
rosparam可以帮助我们将一个yaml格式文件中的参数全部加载到ROS参数服务器中，需要设置command属性为“load”，还可以选择设置命名空间“ns”。
2.2 arg
arg标签用来在launch文件中定义参数，arg和param在ROS里有根本性的区别，就像局部变量和全局变量的区别一样。arg不储存在参数服务器中，不能提供给节点使用，只能在launch文件中使用。param则是储存在参数服务器中，可以被节点使用。
<arg name="demo"/>
像上面这样，就简单地声明了一个参数，名叫demo，但是声明不等于定义，我们需要给他赋值，在赋值之后参数才能够发挥作用。
<arg name="demo1" value="666"/>
<arg name="demo2" default="666"/>
以上是两种简单的赋值方法，两者的区别是使用后者赋值的参数可以在命令行中像下面这样被修改，前者则不行。
roslaunch demo demo.launch demo:2=6666
launch文件中需要使用到argarg-name时，可以使用如下方式调用：
<arg name="arg-name" value="666"/>
<param name="foo" value="$(arg arg-name)" />
<node name="node" pkg="package" type="type "args="$(arg arg-name)" />
当$(arg arg_name)出现在launch文件任意位置时，将会自动替代为所给参数的值。
三、重映射机制
简单来说就是取别名,比如turtlebot的键盘控制节点，发布的速度控制指令话题可能是/turtlebot/cmd_vel，但是我们自己的机器人订阅的速度控制话题是/cmd_vel，这个时候使用remap就可以轻松解决问题，将/turtlebot /cmd_vel重映射为/cmd_vel，我们的机器人就可以接收到速度控制指令了：
<remap from="/turtlebot/cmd_vel" to="/cmd_vel"/>
四、嵌套复用
在复杂的系统当中，launch文件往往有很多，这些launch文件之间也会存在依赖关系。如果需要直接复用一个已有launch文件中的内容，可以使用标签包含其他launch文件，这和C语言中的include几乎是一样的。
<include file="$(find demo)/launch/demo.launch" ns="demo_namespace"/>
属性作用file ="$(find pkg-name)/path/filename.xml"指明我们想要包含进来的文件ns="NAME_SPACE"相对NAME_SPACE命名空间导入文件
五、拓展说明
使用 roslaunch 命令 和 使用 rosrun 命令 单独运行每个节点之间的重要区别
默认情况下，roslaunch 命令 从启动节点开始，标准输出信息会重定向到一个日志文件中，而不会像 rosrun 命令那样，将 log 信息显示在终端(console)上。日志文件所在路径： ∼/.ros/log/run_id/node_name-number-stdout.log
Q： 如何将标准输出信息显示在终端(console)上？
A： 在 node 元素中使用 output 属性：output=”screen”。
扩展： node 元素的 output 属性只能影响这个节点自己。除了 output 属性，我们可以使用 roslaunch命令行工具的 –screen 命令行选项强制性的在终端的窗口中显示所有节点的输出信息。
$ roslaunch --screen package-name launch-file-name
对eigen中的固定大小的类使用STL容器的时候，如果直接使用会出错，所谓固定大小（fixed-size）的类是指在编译过程中就已经分配好内存空间的类，为了提高运算速度，对于SSE或者AltiVec指令集，向量化必须要求向量是以16字节即128bit对齐的方式分配内存空间，所以针对这个问题，容器需要使用eigen自己定义的内存分配器，即aligned_allocator。
#include <Eigen/StdVector>
vector<Eigen::Matrix4d>;
std::map<int, Eigen::Vector4f>
std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>
std::map<int, Eigen::Vector4f, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f>>
或者
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix2d)
std::vector<Eigen::Vector2d>
EIGEN_MAKE_ALIGNED_OPERATOR_NEW:
Eigen内存对齐问题
map的其中一个构造函数有第三个参数，可以直接定义map的key值得排序规则
默认为std::less，即按“<”运算符进行排序
map<string, int> mapWord = { { "father", 1 },{ "mother", 4 },{ "daughter", 5 } };
等价于：
map<string, int, std::less<string>> mapWord2 = { { "father", 1 },{ "mother", 4 },{ "daughter", 5 } };
map<string, int, std::greater<string>> mapWord2 ;
bool compFunc(const string& a, const string& b)
{
return a.compare(b) > 0;
}
map <string, int, decltype(compFunc)*> mapWord3; //注意*号的存在。比较操作类型必须为函数指针类型
auto fc = [](const string& str1, const string& str2) {return str1.compare(str2) > 0; };
map <string, int, decltype(fc)*> mapWord4; //同样要使用使用函数指针
ROS通过image_transport传递通用图像:
参考: image_transport翻译
// Use the image_transport classes instead.
#include <ros/ros.h>
#include <image_transport/image_transport.h>
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
// ...
}
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("in_image_base_topic", 1, imageCallback);
image_transport::Publisher pub = it.advertise("out_image_base_topic", 1);
/*******
image_transport 默认的传输，通过ROS传输sensor_msgs/Image
compressed_image_transport JPEG or PNG 压缩
theora_image_transport 视频流传输
*****/
消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。
举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。
message_filters::Subscriber<sensor_msgs::Image> cam0_img_sub;
message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> stereo_sub;
cam0_img_sub.subscribe(nh, "cam0_image", 10);
cam1_img_sub.subscribe(nh, "cam1_image", 10);
stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);
stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);
参考博客: message_filter用法
struct StateServer {
IMUState imu_state;
CamStateServer cam_states;
// State covariance matrix
Eigen::MatrixXd state_cov;
Eigen::Matrix<double, 12, 12> continuous_noise_cov;
};
每帧特征:
std::map<int, std::vector<FeatureMetaData> > GridFeatures; //key为第几个grid,vector为每个格里的特征集合
struct FeatureMetaData {
FeatureIDType id;
float response;
int lifetime;
cv::Point2f cam0_point;
cv::Point2f cam1_point;
};
image_processor算法介绍
这部分代码也比较简单, Fast特征点加光流,双目相机
kinematics.pdf
常用的四元数类型: Hamilton和JPL
元素顺序不同:
左侧是H,右侧是JPL
H是更习惯俄一种表示方式,JPL类似单应向量的表示,当处理3D空间的几何问题时,使在单应向量上的操作和四元数的操作更一致.
两种方法计算得到的左乘矩阵不同
Hamiltion:
q
=
q
w
+
q
v
q = q_w + \textbf{q}_v
q=qw+qv
JPL:
q
=
q
v
+
q
w
=
q
1
i
+
q
2
j
+
q
3
k
+
q
4
q = \textbf{q}_v + q_w=q_1\textbf{i}+q_2\textbf{j}+q_3\textbf{k}+q_4
q=qv+qw=q1i+q2j+q3k+q4
其次,四元数定义的乘法规则不同, Hamilton是右手系, JPL是左手系, 也就是说, 给定一个旋转轴
u
u
u, 一个按右手系规则旋转
θ
\theta
θ,一个按左手系规则旋转
θ
\theta
θ.所以:
q
r
i
g
h
t
⋆
=
q
l
e
f
t
q_{right}^\star = q_{left}
qright⋆=qleft
所以两种方法对应的旋转矩阵是一个转置的关系:
q4=q0=qw
SO(3)上的加减操作
其中:
四元数与旋转矩阵的对应关系
JPL表示:
Halmtion对应的旋转矩阵是上述矩阵的转置
当旋转的角度很小时,JPL:
如果是Halmition,就把
δ
θ
\delta \theta
δθ变成
−
δ
θ
- \delta \theta
−δθ
雅可比计算(Halmition)
关于向量的Jacobian
2)关于四元数的雅可比
q = [w v], v是向量
SO3的右雅可比
右雅可比有如下性质
而
关于旋转向量的雅可比
四元数的微分
全局扰动
上面使用的JPL表示,也可使用Halmition,
q
=
q
⋆
q=q_\star
q=q⋆,
δ
θ
=
−
δ
θ
\delta \theta = -\delta \theta
δθ=−δθ
局部扰动:
推导使用Halmition,也可使用JPL
四元数积分
零阶四元数积分:
ω
(
t
)
=
ω
\omega(t)=\omega
ω(t)=ω在
δ
t
\delta t
δt内是常值
即
泰勒展开推到得到:
根据:
得到:
一阶积分:
推导结果:
MSCKF统一使用JPL左手系的定义方式, 求解关于扰动的雅可比时,使用左乘的定义方式.