一、本文开发环境
1. Ubuntu 18.04
2. ROS Melodic（注意：ROS版本须与Ubuntu版本对应，否则安装会出现问题）
3. RoboWare Studio（非必需）
4. 悉灵相机MV-EB435i
5. SDK_Mv3dRgbd_V1.0.0_Linux_220622_837455
二、ROS简介
ROS（Robot Operating System）是一个适用于机器人的开源的元操作系统。它提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及包管理。它也提供用于获取、编译、编写、和跨计算机运行代码所需的工具和库函数。
三、ROS通信机制
ROS一个核心的概念叫做节点，一个节点也可以理解为一个进程，可以利用不同节点共同协作来一个完成一个庞大的任务，其中每个负责一个相对独立的部分，例如本课题，可以通过一个节点启动维护相机并从相机获取图像数据发送给其他接单，而其他节点基于相机数据完成任务。
不同节点之间需要通过一定的机制进行通信，ROS 最常用的通信方式有三种：话题（Topic）通信、服务（Service）通信和 共享参数，其中话题通信是异步数据流通信，而服务通信是同步数据流通信，当然还有其他的通信方式，例如节点之间的 Nodelet 通信。
由于本课题的数据只要是基于相机的数据，因此采用异步数据流通信即话题通信更为合适。另外，为了提高实时性，可以采用Nodelet通信方式进行相机数据的传输。
四、ROS下集成悉灵相机并获取图像数据
（一）基于话题通信方式
在话题（Topic）通信机制里，主要有三个角色：发布者，订阅者，Master；根据 Master 的参与主要分为两个阶段：连接前的准备，连接和通信.
1. 第一阶段：建立前的准备
在建立连接阶段，主要是解决发布者与订阅者进行匹配进而连接的问题，在这个阶段主要分为五步：
（1）发布者（Talker）启动，通过RPC向ROS Master注册发布者的信息，包括：发布者节点信息，话题名，话题缓存大小等；Master会将这些信息加入注册列表中；
（2）订阅者（Listener）启动，通过RPC向ROS Master注册订阅者信息，包括：订阅者节点信息，话题名等；Master会将这些信息加入注册列表；
（3）Master 进行节点匹配：Master 会根据订阅者提供的信息，在注册列表中查找匹配的发布者；如果没有发布者（Talker），则等待发布者（Talker）的加入；如果找到匹配的发布者（Talker），则会主动把发布者（Talker）（有可能是很多个 Talker）的地址通过RPC传送给订阅者（Listener）节点；
（4）Listener 接收到 Master 的发出的Talke 的地址信息，尝试通过RPC向Talker发出连接请求（信息包括：话题名，消息类型以及通讯协议（TCP/UDP））；
（5）Talker收到Listener发出的连接请求后，通过RPC向Listener确认连接请求（包含的信息为自身TCP地址信息）；
至此，Talker和Listener做好了连接前的准备工作。在这个过程中，Master起到了牵线搭桥的作用；
2. 第二阶段：连接和通信
在发布消息阶段，主要解决的是发布者(Talker) 如何发布消息进入传递给订阅者(Listener)的过程。在这个过程中，完全是Talker和Listener两者之间的信息单向流动，Master 并未参与其中，主要分为两步：
（1）Listener接收到Talker的确认消息后，使用TCP尝试与Talker建立网络连接；
（2）成功连接之后，Talker开始向Listener发布话题消息数据；
至此，完成了Talker向Listener发布消息的过程；Master在这个阶段并不参与两者之间的数据传递。
需要注意的是，有可能多个Talker连接一个Listener，也有可能是一个Talker连接上多个Listener。
（二）基于nodelet通信方式
本文主要介绍基于Nodelet通信方式的实现，因为Nodelet能够提供一种在单机器单进程运行多个算法而不会在进程中传递消息时产生复制成本的方法，这正好符合图像传输时数据量大，实时性要求高的特点。基本原理就是让多个算法程序在一个进程中用shared_ptr实现零拷贝通信（zero copy transport），以降低因为传输大数据而损耗的时间。Nodelet可以将多个node捆绑在一起管理，使得同一个manager里面的topic的数据传输更快。
实现过程
1. 概述
Nodelet节点与传统的ros节点不同，其源文件中不存放main函数，而是定义一个类，在编译时，不是编译成可执行文件，而是编译成库文件。
2. 项目package文件树
3. 创建package
在catkin_ws目录下，创建package
<![CDATA[catkin_create_pkg base_nodelet]]>
4. 分别创建include、lib、src、plugins、launch文件夹
<![CDATA[cd ./catkin_ws/
mkdir include lib src plugins launch]]>
把SDK中的头文件和库文件分别放入include和lib文件夹
5. 转到src文件夹下，编写源代码
<![CDATA[cd src]]>
创建文件nodeletclass1.h
<![CDATA[ #include <nodelet/nodelet.h>]]><![CDATA[ #include <pluginlib/class_list_macros.h>
#include <ros/ros.h>]]><![CDATA[ #include <image_transport/image_transport.h>]]><![CDATA[ #include <cv_bridge/cv_bridge.h>]]><![CDATA[ #include <sensor_msgs/image_encodings.h>]]><![CDATA[ #include <opencv2/highgui/highgui.hpp>]]><![CDATA[ #include "../include/base_nodelet/common.hpp"]]><![CDATA[ #include <sensor_msgs/Image.h>]]><![CDATA[
class nodeletclass1 :public nodelet::Nodelet // 继承父类nodelet::Nodelet
{
public:
nodeletclass1();
public:
// 这个虚函数，在启动本Nodelet节点时，自动调用
virtual void onInit();
// 用于YUV422转BGR
static int YUYVToBGR24_Native(unsigned char* pYUV, int width, int height);
static unsigned char* pBGR24;
};]]>
创建文件nodeletclass1.cpp
<![CDATA[ #include "nodeletclass1.h"
nodeletclass1::nodeletclass1()
{
}
//重载虚函数，启动时自动调用
void nodeletclass1::onInit()
{
/*
连接并启动相机，略
*/
// 设置图片节点
image_transport::ImageTransport it(nh);
// 设置图片的发布者，第一个参数是话题的名称，第二个是缓冲区的大小（即消息队列的长度，在发布图像消息时消息队列的长度只能是1）
image_transport::Publisher rgb_pub = it.advertise("rgb/image_raw", 1);
image_transport::Publisher depth_pub = it.advertise("depth/image_raw", 1);
image_transport::Publisher lir_pub = it.advertise("lir/image_raw", 1);
image_transport::Publisher rir_pub = it.advertise("rir/image_raw", 1);
// 设置主题的发布频率
ros::Rate loop_rate(30);
// 图片节点进行主题的发布
while(ros::ok())
{
int nRet = MV3D_RGBD_FetchFrame(handle, &stFrameData, 5000);
if (MV3D_RGBD_OK == nRet)
{
for (int i = 0; i < stFrameData.nImageCount; i++)
{
// 打印图像信息
LOGD("MV3D_RGBD_FetchFrame success: framenum (%d) height(%d) width(%d) len (%d)!", stFrameData.stImageData[i].nFrameNum,
stFrameData.stImageData[i].nHeight, stFrameData.stImageData[i].nWidth, stFrameData.stImageData[i].nDataLen);
// 发布RGB图
if (ImageType_YUV422 == stFrameData.stImageData[i].enImageType)
{
// 将yuv422格式转换为BGR格式
YUYVToBGR24_Native(stFrameData.stImageData[i].pData, stFrameData.stImageData[i].nWidth, stFrameData.stImageData[i].nHeight);
LOGD("YUYVToBGR24_Native success.");
// 将图像转成OpenCV中的mat格式
cv::Mat image(stFrameData.stImageData[i].nHeight, stFrameData.stImageData[i].nWidth, CV_8UC3, pBGR24);
// 利用cv_bridge库将mat格式转换为ROS中的sensor_msgs::Image格式
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
// 发布图像数据
rgb_pub.publish(*imageMsg);
}
// 发布深度图
if (ImageType_Depth == stFrameData.stImageData[i].enImageType)
{
cv::Mat image(stFrameData.stImageData[i].nHeight, stFrameData.stImageData[i].nWidth, CV_8UC2, stFrameData.stImageData[i].pData);
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_16UC1, image).toImageMsg();
depth_pub.publish(*imageMsg);
}
// 发布左IR图
if (i == 2)
{
cv::Mat image(stFrameData.stImageData[i].nHeight, stFrameData.stImageData[i].nWidth, CV_8UC1, stFrameData.stImageData[i].pData);
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", image).toImageMsg();
lir_pub.publish(*imageMsg);
}
// 发布右IR图
if (i == 3)
{
cv::Mat image(stFrameData.stImageData[i].nHeight, stFrameData.stImageData[i].nWidth, CV_8UC1, stFrameData.stImageData[i].pData);
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", image).toImageMsg();
rir_pub.publish(*imageMsg);
}
}
}
ros::spinOnce();
// 按照设定的频率来将程序挂起
loop_rate.sleep();
}
/*
断开连接并释放相机，略
*/
}
//nodelet的本质是把节点作为插件来调用，因此需要PLUGINLIB的宏定义，通常放在cpp文件结尾
//第一个参数是类名，第二个参数是父类
PLUGINLIB_EXPORT_CLASS(nodeletclass1, nodelet::Nodelet);]]>
6. 进入plugins文件夹，创建插件的引用xml文件
<![CDATA[cd plugins]]>
创建nodelet_plugins.xml
<![CDATA[ <!--这里的path="",修改成path="lib/lib{项目名}",
项目名就是CMakeLists.txt里面定义的project(base_nodelet)
我这里就是path="lib/libbase_nodelet"
-->
<library path="lib/libbase_nodelet" >
<!-- name: launch文件里面 load 后面接着的插件名
type: c++文件定义的类名
如 name="aaa/nodeletclass1",那么，launch文件对应启动如下：
<node pkg="nodelet" type="nodelet" name="nodeletclass1"
args="load aaa/nodeletclass1 nodelet_manager" output="screen">
-->
<class name="aaa/nodeletclass1" type="nodeletclass1" base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
</library>]]>
7. 回到项目根目录
<![CDATA[cd base_nodelet]]>
修改package.xml文件
添加对其他package的依赖：
<![CDATA[ <build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<exec_depend>nodelet</exec_depend>
<exec_depend>roscpp</exec_depend>]]>
在<export></export>标签中间加入如下内容：
<![CDATA[ <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />]]>
8. 编写CMakeLists.txt
由于Nodelet不是编译成可执行文件，而是库文件。
所以加入如下语句：
<![CDATA[add_library(${PROJECT_NAME} src/nodeletclass1.cpp)]]>
同时加入如下语句：
<![CDATA[ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
/opt/ros/melodic/lib/libimage_transport.so #
${PROJECT_SOURCE_DIR}/lib/libMv3dRgbd.so
)]]>
9. 编写launch文件
进入launch目录
<![CDATA[cd launch]]>
创建nodeletclass1.launch
<![CDATA[ <launch>
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="nodeletclass1" args="load aaa/nodeletclass1 nodelet_manager" output="screen">
</node>
</launch>]]>
launch文件启动了两个节点
一个是nodelet manager，并重命名为 nodelet_manager
另外一个则是编写完成的Nodelet节点插件
10. 编译与测试
添加环境环境变量（注意必须添加到root用户下的环境变量，因为USB相关的读写操作需要root权限，sudo也不行）
<![CDATA[export LD_LIBRARY_PATH=/UserDemoPath:$LD_LIBRARY_PATH]]>
转入工作空间根目录
<![CDATA[catkin_make]]>
编译完成，启动终端后切换到root用户，执行：
<![CDATA[source devel/setup.bash]]>
启动launch文件
<![CDATA[roslaunch base_nodelet nodeletclass1.launch]]>
查看当前话题列表
<![CDATA[rostopic list]]>
可以看到成功发布了四种图像格式的话题
打开Rviz，订阅图像话题
<![CDATA[rviz]]>
浙公网安备 33010802013223号