Excerpt |
---|
本页介绍了导航方案添加深度摄像头的用法, 主要包含对深度摄像头参数的配置以及publishDepthCamFrame接口的调用。 |
本页内容
Table of Contents |
---|
1111
1
概述
结合模块导航方案,使用深度摄像头时,应该将深度摄像头安装在上位机上。用户自行编写摄像头的驱动程序,在获取其深度数据后,再通过我们SDK中提供的publishDepthCamFrame接口将传输给模块,至此Slamware Core才能将深度数据用于避障。
配置连接到用户自己的上位机上,并由用户自行编写摄像头的驱动程序,在获取其深度数据后,再通过我们SDK中提供的publishDepthCamFrame接口将传输给Slamware Core或slamware Cube,这样才能将深度数据用于避障。
配置传感器(适用于core)
同添加其它传感器类似,在安装深度摄像头后,需通过RoboStudio中的Slamware配置工具将深度摄像头的具体安装位姿等相关信息进行配置,然后将配置好的文件导出,重新放到底盘MCU中进行Make,再将生成的新的hex文件烧录至底盘。
开启滤波:高度滤波开关,只处理相应高度范围内的障碍物测距信息,主要应用于滤除地面噪声和高于机器人高度的障碍物信息。
开启形态学滤波:利用形态学运算主要采用态学开、闭运算进行滤波操作,用来过滤孤立噪点。(可能会影响性能,客户自行取舍是否勾选)
倒装:摄像头是否倒置,一般不勾选。
最小滤波高度:以地面为基准,高度小于该值的障碍物测距信息将被忽略,单位为m。
最大滤波高度:以地面为基准,高度大于该值的障碍物测距信息将被忽略,单位为m。
相关文档:KBSW180147 机器人底盘传感器位置坐标及底盘配置工具的使用
Info | ||
---|---|---|
| ||
配置深度摄像头前,请先确保使用的固件版本至少需要2.6.7及以上,否则配置的滤波相关信息无法被读出。 |
配置传感器(适用于cube)
具体使用方法可咨询售后技术支持
接口调用
sensorId:使用Slamware配置工具配置时,设置的Id.
DepthCameraFrame:需要传入一个DepthCameraFrame类型的结构体变量,代表的是一帧数据。
DepthCameraFrame的成员:
深度摄像头有自己的属性参数,如分辨率,深度视角(FOV)等。定义DepthCameraFrame类型的结构体变量时,可以把深度摄像头的属性参数赋值给成员。另外,请注意如下几点:
- 注意各个成员的单位,距离为米,角度为弧度。
- maxValidDistance建议在2米,否则可能存在噪点。
- cols和rows,建议为320x240,不要大于这个值。如果一定要设置为更小的分辨率,需先确认所用的摄像头是否支持。
- minFovPitch和maxFovPitch涉及到深度摄像头的VFOV(垂直视角)。假设VFOV=a(rad),则minFovPitch=-a/2,maxFovPitch=a/2。
- minFovYaw和maxFovYaw涉及到深度摄像头的HFOV(水平视角)。假设HFOV=b(rad),则minFovPitch=-b/2,maxFovPitch=b/2。data就是具体的一帧里面每个像素的值,需要把完整的一帧的深度数据全传进去。
- data就是具体的一帧里面每个像素的深度距离值,需要把完整的一帧的深度数据全传进去。
代码描述
Code Block | ||||||
---|---|---|---|---|---|---|
| ||||||
#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
frame.data.swap(frame_buffer);
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.对于深度摄像头的规格型号,并无特殊要求,只需要在调用接口时传送符合要求的数据即可。注意,需要的深度信息单位是米,如果您的摄像头的单位与此不同,需要转换。
34.对于不同品牌型号的深度摄像头,驱动摄像头的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. 示例程序
Code Block | ||||||||
---|---|---|---|---|---|---|---|---|
| ||||||||
#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库文件
Code Block | ||||||||
---|---|---|---|---|---|---|---|---|
| ||||||||
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文件夹,使用如下指令运行即可
Code Block | ||||
---|---|---|---|---|
| ||||
$ ./use_depth_cam_demo 192.168.11.1 |
当成功传输了正确符合要求的深度数据后,如果深度摄像头测距范围内有障碍物,可以通过RoboStudio看到深度信息。
depthcam_publish_demo.cpp是一个非常简单的示例文件,DepthCameraFrame中用到的data是人为构造的一个vector,用户可以自行下载,查看检测到深度数据的效果。