本页介绍了导航方案添加深度摄像头的用法, 主要包含对深度摄像头参数的配置以及publishDepthCamFrame接口的调用。



本页内容


概述

结合模块导航方案,使用深度摄像头时,应该将深度摄像头连接到用户自己的上位机上,并由用户自行编写摄像头的驱动程序,在获取其深度数据后,再通过我们SDK中提供的publishDepthCamFrame接口将传输给Slamware Core或slamware Cube,这样才能将深度数据用于避障。



配置传感器(适用于core)

同添加其它传感器类似,在安装深度摄像头后,需通过RoboStudio中的Slamware配置工具将深度摄像头的具体安装位姿等相关信息进行配置,然后将配置好的文件导出,重新放到底盘MCU中进行Make,再将生成的新的hex文件烧录至底盘。

开启滤波:高度滤波开关,只处理相应高度范围内的障碍物测距信息,主要应用于滤除地面噪声和高于机器人高度的障碍物信息。
开启形态学滤波:利用形态学运算主要采用态学开、闭运算进行滤波操作,用来过滤孤立噪点。(可能会影响性能,客户自行取舍是否勾选)
倒装:摄像头是否倒置,一般不勾选。
最小滤波高度:以地面为基准,高度小于该值的障碍物测距信息将被忽略,单位为m。
最大滤波高度:以地面为基准,高度大于该值的障碍物测距信息将被忽略,单位为m。

相关文档:KBSW180147 机器人底盘传感器位置坐标及底盘配置工具的使用 

                  KBSW180158 Breakout kit 烧写固件

配置深度摄像头前,请先确保使用的固件版本至少需要2.6.7及以上,否则配置的滤波相关信息无法被读出。


配置传感器(适用于cube)

具体使用方法可咨询售后技术支持



接口调用

sensorId:使用Slamware配置工具配置时,设置的Id.
DepthCameraFrame:需要传入一个DepthCameraFrame类型的结构体变量,代表的是一帧数据。
DepthCameraFrame的成员:

深度摄像头有自己的属性参数,如分辨率,深度视角(FOV)等。定义DepthCameraFrame类型的结构体变量时,可以把深度摄像头的属性参数赋值给成员。另外,请注意如下几点:

  1. 注意各个成员的单位,距离为米,角度为弧度。
  2. maxValidDistance建议在2米,否则可能存在噪点。
  3. cols和rows,建议为320x240,不要大于这个值。如果一定要设置为更小的分辨率,需先确认所用的摄像头是否支持。
  4. minFovPitch和maxFovPitch涉及到深度摄像头的VFOV(垂直视角)。假设VFOV=a(rad),则minFovPitch=-a/2,maxFovPitch=a/2。
  5. minFovYaw和maxFovYaw涉及到深度摄像头的HFOV(水平视角)。假设HFOV=b(rad),则minFovPitch=-b/2,maxFovPitch=b/2。
  6. data就是具体的一帧里面每个像素的深度距离值,需要把完整的一帧的深度数据全传进去。



代码描述

#include <rpos/robot_platforms/slamware_core_platform.h>

struct CamAttr
{ 
    float minValidDistance;
    float maxValidDistance;
    float minFovPitch;
    float maxFovPitch;
    float minFovYaw;
    float maxFovYaw;
    int cols;
    int rows;
};

int main(int argc, char* argv[])
{ 
    try 
    { 
        rpos::robot_platforms::SlamwareCorePlatform platform = rpos::robot_platforms::SlamwareCorePlatform::connect("192.168.11.1", 1445); 

        CamAttr camera_attr; 
        getAttrFromDevice(camera_attr);        // TODO: Get attr from device doc maybe

        rpos::message::depth_camera::DepthCameraFrame frame;
        frame.minValidDistance = camera_attr.minValidDistance;
        frame.maxValidDistance = camera_attr.maxValidDistance;
        frame.minFovPitch = camera_attr.minFovPitch;
        frame.maxFovPitch = camera_attr.maxFovPitch;
        frame.minFovYaw = camera_attr.minFovYaw;
        frame.maxFovYaw = camera_attr.maxFovYaw;
        frame.cols = camera_attr.cols;
        frame.rows = camera_attr.rows;

        std::vector<float> frame_buffer;
        getFrameFromDepthCam(frame_buffer);    // TODO: Get Frame buffer from DepthCam Device
        

        int sensorId = 1; //TODO: Config your own SensorId 
        platform.publishDepthCamFrame(sensorId, frame); 
    } 
    catch(const std::exception& ex)
    { 
        std::string ex_str(ex.what()); 
    }
    return 0;
}

上述代码仅传输了一帧数据。实际在调用接口传递数据时,需要注意以下几点:

1.深度摄像头的数据是要持续发送的。为了防止数据传输堵塞,调用publishDepthCamFrame的频率不要超过一秒钟10次。

2.得到深度数据后,模式是实时处理的,为了防止有延迟,请选择性能好的上位机,获取一帧深度数据到调用publishDepthCamFrame接口总的时长应该在100ms以下。

3.对于深度摄像头的规格型号,并无特殊要求,只需要在调用接口时传送符合要求的数据即可。注意,需要的深度信息单位是米,如果您的摄像头的单位与此不同,需要转换。

4.对于不同品牌型号的深度摄像头,驱动摄像头的SDK不同,若对此需要帮助,建议您咨询摄像头品牌的支持工程师以获得专业的支持。



检验效果

当成功传输了正确符合要求的深度数据后,如果深度摄像头测距范围内有障碍物,可以通过RoboStudio或者Web portal看到深度信息。

depthcam是一个非常简单的示例文件,DepthCameraFrame中用到的data是人为构造填充的一个vector,用户可以自行下载,在VS2010中打开,查看检测到深度数据的效果。

1. 通过RoboStudio查看

在此之前,需先获取下载Slamware Sensor Map插件(参考RoboStudio用户手册)。点击工作区的显示设置图标,在弹出的图层面板中选中传感器视野(深度摄像头)

2. 通过Web Portal查看

访问Web portal,开启Slamware Core诊断模式,具体步骤请参考KBSW180153 SLAMWARE Web Portal Function Overview 

示例1:使用Intel Realsense D435i 深度相机(Windows SDK)

1. 安装librealsense2

从Intel官方网站下载D435i对应驱动文件,具体可参照官方教程安装最新版librealsense2 。

2. 下载slamware sdk文件

从slamtec官网下载最新版vs2017版本的sdk文件包,并解压。

3. 新建VS2017工程文件

由于librealsense2的sdk建议使用的是VS2015版本以上进行编译,同时slamware的windows sdk目前只支持VS2010和VS2017两个版本,因此使用VS2017来进行开发。

打开VS2017,依次选择file → new → project → Visual C++ → Empty project,自定义工程文件名和存放位置。

在创建好的项目中,右键Source Files → add → new item → Visual C++ → C++ files(.cpp),自定义cpp文件名称。

创建好后如下图所示

4. 设置头文件和依赖库

这里可根据需要设置头文件和依赖库的作用范围,示例选择将realsense SDK的作用范围限制在当前项目,将slamware SDK的应用于整个解决方案中。

在项目名称上右击打开Properties,在顶部的平台设置中选择Win32。

点击VC++Directories,分别修改Include Directories和Library Directories,Include Directories选择slamware SDK中的include文件夹,Library Directories选择slamware SDK中的lib文件夹,如下图

再点击Properties的C/C++修改General中的Additional Include Directories,添加之前安装的realsense SDK中的include路径

再点击Properties的Linker修改General中的Additional Library Directories,添加之前安装的realsense SDK中的lib/x86路径,同时在Linker下的Input中的Additional Dependencies中手动添加realsense2.lib。

5. 示例程序


#include <regex>
#include <iostream>
#include <boost/thread/thread.hpp>

#include <rpos/robot_platforms/slamware_core_platform.h>  //Include slamware API

#include <librealsense2/rs.hpp>  //Include RealSense Cross Platform API

std::string ip_address = "";
const char *ip_regex = "\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}";
rpos::robot_platforms::SlamwareCorePlatform sdp;

void ShowHelp(std::string app_name) {
	std::cout << "SLAMWARE console demo." << std::endl <<
		"Usage: " << app_name << " <slamware_address>" << std::endl;
}

bool ParseCommandLine(int argc, const char *argv[]) {
	bool opt_show_help = false;
	for (int pos = 1; pos < argc; ++pos) {
		const char *current = argv[pos];
		if (strcmp(current, "-h") == 0)
			opt_show_help = true;
		else
			ip_address = current;
	}
	std::regex reg(ip_regex);
	if (!opt_show_help && !std::regex_match(ip_address, reg))
		opt_show_help = true;
	if (opt_show_help) {
		ShowHelp("use_depth_camera_demo");
		return false;
	}
	return true;
}

int main(int argc, const char *argv[]) {
	if (!ParseCommandLine(argc, argv)) return 1;
	std::cout << "Connecting SDP @ " << ip_address << "..." << std::endl;
	try {
		//connect device
		sdp = rpos::robot_platforms::SlamwareCorePlatform::connect(ip_address, 1445);
		std::cout << "SDK Version: " << sdp.getSDKVersion() << std::endl;
		std::cout << "SDP Version: " << sdp.getSDPVersion() << std::endl;

		//this id should be same as you set the cam info via RS config tool
		int depthCamSensorId = 5;
		//create a depth msg frame
		rpos::message::depth_camera::DepthCameraFrame _frame;
		_frame.minValidDistance = 0.2f;
		_frame.maxValidDistance = 3.0f;
		_frame.minFovPitch = -30 * 3.14 / 180;
		_frame.maxFovPitch = 30 * 3.14 / 180;
		_frame.minFovYaw = -45 * 3.14 / 180;
		_frame.maxFovYaw = 45 * 3.14 / 180;
		_frame.cols = 320;
		_frame.rows = 240;
		//init data vector
		_frame.data.resize(_frame.cols * _frame.rows, 0.0f);

		// Create a Pipeline - this serves as a top-level API for streaming and processing frames
		rs2::pipeline p;
		// Configure and start the pipeline
		p.start();

		//process data
		while (true) {
			// Block program until frames arrive
			rs2::frameset rs_frames = p.wait_for_frames();
			// Try to get a frame of a depth image
			rs2::depth_frame _depth_frame = rs_frames.get_depth_frame();
			// Get the depth frame's dimensions
			int width = _depth_frame.get_width();
			int height = _depth_frame.get_height();
			std::cout << "x: " << width << " y: " << height << std::endl;
			auto it = _frame.data.begin();
			for (int i = (height - _frame.rows) / 2; i < height - (height - _frame.rows) / 2; i++) {
				for (int j = (width - _frame.cols) / 2; j < width - (width - _frame.cols) / 2; j++) {
					*it = _depth_frame.get_distance(j, i);
					it++;
				}
			}
			sdp.publishDepthCamFrame(depthCamSensorId, _frame);
			std::cout << "publish a depth frame..." << std::endl;

			//control the publish rate less than 10HZ
			boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
		}
		return EXIT_SUCCESS;
	}
	catch (const rpos::system::detail::ExceptionBase& e) {
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}
	catch (const rs2::error & e)
	{
		std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
		return EXIT_FAILURE;
	}
	catch (const std::exception& e)
	{
		std::cerr << e.what() << std::endl;
		return EXIT_FAILURE;
	}
}


6. 编译和运行

使用有线连接机器人,realsense连接到电脑的USB3.0口

编译工程。

运行前,在项目的Properties中点击Debugging,在Command Arguments中添加设备IP,默认192.168.11.1

如运行报错,缺少realsense2.dll,请将realsense SDK的安装目录中bin/x86下的realsense2.dll复制到项目目录,与编译生成的可执行文件放置在同一目录下即可。

示例2:使用Intel Realsense D435i 深度相机(Linux SDK)

1. 安装librealsense2

从Intel官方网站下载D435i对应驱动文件,具体可参照官方教程安装最新版librealsense2 。

2. 下载slamware sdk文件

从slamtec官网下载最新版Linux版本的sdk文件包,并解压。

3. 创建文件

为方便编译,我们直接在slamware SDK文件夹下新建文件,首先进入SDK中的samples文件夹下,创建use_depth_cam_demo文件夹,并在文件夹下创建一个Makefile文件和src文件夹,src文件夹下再创建main.cpp文件,具体文件结构如下图

4. 代码示例

main.cpp代码同window示例

5. 编译和运行

打开之前新建的use_depth_cam_demo/Makefile文件,根据其他示例编写方法,链接原有的slamware SDK,并添加librealsense2库文件

HOME_TREE := ../../

MODULE_NAME := $(notdir $(CURDIR))

CXXSRC 	   := src/main.cpp
CXXFLAGS   := -std=c++0x
LDFLAGS    := -Wl,-Bdynamic -lrealsense2

LD_LIBS    =  -Xlinker "-(" \
              -lstdc++ -ldl -lrt -lm -lpthread \
              -l$(RPOS_SLAMWARE_NAME) -l$(RPOS_CORE_NAME) \
              -lboost_atomic \
              -lboost_chrono \
              -lboost_date_time \
              -lboost_regex \
              -lboost_system \
              -lboost_thread \
              -lboost_filesystem \
              -lboost_random \
              -lbase64 \
              -ljsoncpp \
              -lrlelib \
              -lcrypto \
              -lcurl \
              -lssl \
              -Xlinker "-)" \

all: build_app

clean: clean_app

#includes the common definitions...
#do not remove them unless you know what you are doing
include $(HOME_TREE)/mak_def.inc
include $(HOME_TREE)/mak_common.inc

打开终端,进入use_depth_cam_demo文件夹,使用make指令进行编译(注意使用gcc和g++的版本要都为4.9)。

编译完成后,进入linux-x86_64-release/output文件夹,使用如下指令运行即可

$ ./use_depth_cam_demo 192.168.11.1