ROS-OpenCV
1. 环境配置
1.1 realsense SDK2.0安装
①通过官网找到最新的SDK包并下载
Intel RealSense SDK 2.0
②解压安装包(librealsense-2.47.0.tar.gz)
③注册服务器的公钥:
sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
④将服务器添加到存储库列表中:
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
⑤安装所用的库
sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils
⑥可选安装开发和调试包(最好全部安装)
sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg
⑦检查内核是否更新应包含realsense字符串
modinfo uvcvideo | grep "version:"
⑧重新链接摄像头运行:realsense-viewer
打开摄像头
1.2 摄像头打开失败
①检查内核是否更新应包含realsense字符串
modinfo uvcvideo | grep "version:"
②可出现内核版本则是驱动没有安装
1.3 realsense驱动安装
参考信息:https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md
①安装环境
- 更新 Ubuntu 发行版,包括获取最新的稳定内核
sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade
- 更新操作系统启动并重新启动以强制执行正确的内核选择
sudo update-grub && sudo reboot
②拔下任何连接的摄像头
③构建librealsense2 SDK
- 导航到ibrealsense根目录并运行
mkdir build && cd build
- 运行Cmake:
cmake ../ -DBUILD_EXAMPLES=true
- 重新编译并安装ibrealsense(开启8线程编译,8倍的快乐)
sudo make uninstall && make clean && make -j8&& sudo make install
- 安装完成既可以打开摄像头
④测试摄像头
- 打开librealsense根目录下的demo程序
cd librealsense/examples/capture
- 打开程序rs-capture
rs-capture
⑤能打开深度图与彩色图则安装完成
1.4 ROS-OpenCV安装
①OpenCV官网
- 官网链接如下:
https://opencv.org/releases/page/3/
在版本下找OpenCV4.5.3 - 下载解压文件到特定文件夹中
②进入OpenCV文件夹
-
cd opencv-4.5.3
- 安装依赖库
sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev libavformat-dev libjpeg.dev libtiff5.dev libswscale-dev libjasper-dev
- 在OpenCV下创建编译文件夹
mkdir build
cd build
- 进行文件配置
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local/opencv453 ..
- 进行源码包的编译
sudo make -j8
- 对OpenCV-4.5.3进行安装
sudo make install
③对OpenCV进行配置
④尝试打开OpenCV-4.5.3文件下的例程
cd opencv-4.5.3/samples/cpp/example_cmake
- 编译文件
make
- 出现opencv_example,打开程序可以看到经典的Hello OpenCV
./opencv_example
- 若编译失败,可能是路径问题,查询路径是否出现opencv-4.5.3/include/opencv4/opencv2,将opencv2移动到include中,顺便删除opencv4
- 若编译找不到opencv2/***文件时,如上移动文件
⑤ROS配置OpenCV
2. 使用ROS中的OpenCV开发
2.1 创建ROS工作包环境
①生成ROS的依赖包
cd /catkin_ws/src
catkin create pkg YOUR_PKG_NAME --catkin-deps roscpp rospy
cd ~/catkin_ws
source ~/.bashrc
catkin build
②在生成的包下新建新的cpp文件,写入测试代码
参考文献:realsense SDK2.0学习
#include "iostream"
#include "sstream"
#include "fstream"
#include "algorithm"
#include "cstring"
using namespace std;
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/highgui/highgui_c.h"
using namespace cv;
#include "librealsense2/rs.hpp"
#include "librealsense2/rsutil.h"
float get_depth_scale(const rs2::device& dev)
{
for (rs2::sensor & sensor : dev.query_sensors())
{
if(rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
{
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device Error!");
}
Mat align_Depth2Color(Mat depth, const Mat &color, rs2::pipeline_profile profile) {
auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
const auto intrinDepth = depth_stream.get_intrinsics();
const auto intrinColor = color_stream.get_intrinsics();
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
float pd_uv[2],pc_uv[2];
float Pdc3[3],Pcc3[3];
float depth_scale = get_depth_scale(profile.get_device());
int y,x;
Mat result = Mat(color.rows,color.cols,CV_16U,Scalar(0));
for(int row=0;row<depth.rows;row++)
{
for(int col=0;col<depth.cols;col++)
{
pd_uv[0] = col;
pd_uv[1] = row;
uint16_t depth_value = depth.at<uint16_t>(row,col);
float depth_m = depth_value*depth_scale;
rs2_deproject_pixel_to_point(Pdc3,&intrinDepth,pd_uv,depth_m);
rs2_transform_point_to_point(Pcc3,&extrinDepth2Color,Pdc3);
rs2_project_point_to_pixel(pc_uv,&intrinColor,Pcc3);
x = (int )pc_uv[0];
y = (int )pc_uv[1];
x = x < 0 ? 0 : x;
x = x > depth.cols-1 ? depth.cols-1 : x;
y = y < 0 ? 0 : y;
y = y > depth.rows-1 ? depth.rows-1 : y;
result.at<uint16_t>(y,x)=depth_value;
}
}
return result;
}
void measure_distance(Mat &color,Mat depth,cv::Size range,rs2::pipeline_profile profile)
{
float depth_scale = get_depth_scale(profile.get_device());
cv::Point center(color.cols/2,color.rows/2);
cv::Rect RectRange(center.x-range.width/2,center.y-range.height/2,range.width,range.height);
float distance_sum = 0;
int effective_pixel = 0;
for(int y = RectRange.y;y < RectRange.y + RectRange.height;y++)
{
for(int x = RectRange.x;x < RectRange.x + RectRange.width;x++)
{
if(depth.at<uint16_t>(y,x))
{
distance_sum += depth_scale*depth.at<uint16_t>(y,x);
effective_pixel++;
}
}
}
cout << "有效像素点:" << effective_pixel <<endl;
float effective_distance = distance_sum/effective_pixel;
cout << "目标距离:" << effective_distance << "m" << endl;
char distance_str[30];
sprintf(distance_str,"The distance is:%f m",effective_distance);
cv::rectangle(color,RectRange, Scalar(0,0,255),2,8);
cv::putText(color,(string)distance_str,cv::Point(color.cols*0.02,color.rows*0.05),
cv::FONT_HERSHEY_PLAIN,2,Scalar(0,255,0),2,8);
}
int main()
{
const char* depth_win = "depth_Image";
namedWindow(depth_win,WINDOW_AUTOSIZE);
const char* color_win = "color_Image";
namedWindow(color_win,WINDOW_AUTOSIZE);
rs2::colorizer c;
rs2::pipeline pipe;
rs2::config pipe_config;
pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
rs2::pipeline_profile profile = pipe.start(pipe_config);
float depth_clipping_distance = 1.f;
auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
auto intrinDepth = depth_stream.get_intrinsics();
auto intrinColor = color_stream.get_intrinsics();
auto extrinDepth2Color = depth_stream.get_extrinsics_to(color_stream);
while (cvGetWindowHandle(depth_win)&&cvGetWindowHandle(color_win))
{
rs2::frameset frameset = pipe.wait_for_frames();
rs2::frame color_frame = frameset.get_color_frame();
rs2::frame depth_frame = frameset.get_depth_frame();
rs2::frame depth_frame_4_show = frameset.get_depth_frame().apply_filter(c);
const int depth_w = depth_frame.as<rs2::video_frame>().get_width();
const int depth_h = depth_frame.as<rs2::video_frame>().get_height();
const int color_w = color_frame.as<rs2::video_frame>().get_width();
const int color_h = color_frame.as<rs2::video_frame>().get_height();
Mat depth_image(Size(depth_w, depth_h),
CV_16U, (void *) depth_frame.get_data(), Mat::AUTO_STEP);
Mat depth_image_4_show(Size(depth_w, depth_h),
CV_8UC3, (void *) depth_frame_4_show.get_data(), Mat::AUTO_STEP);
Mat color_image(Size(color_w,color_h),
CV_8UC3,(void*)color_frame.get_data(),Mat::AUTO_STEP);
Mat result = align_Depth2Color(depth_image,color_image,profile);
measure_distance(color_image,result,cv::Size(20,20),profile);
imshow(depth_win,depth_image_4_show);
imshow(color_win,color_image);
waitKey(1);
}
return 0;
}
- 在ROS依赖包中的CMakeLists.txt中添加OpenCV依赖:
cmake_minimum_required(VERSION 3.16.3)
project(measure_distance)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
OpenCV REQUIRED
roscpp
rospy
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES measure_distance
# CATKIN_DEPENDS roscpp rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(${OpenCV_INCLUDE_DIRS})
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/measure_distance.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(realsense_distance src/realsense_distance.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(realsense_distance ${OpenCV_LIBS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_measure_distance.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
#添加后进行调试
set( CMAKE_BUILD_TYPE Debug )
set( DEPENDENCIES realsense2)
target_link_libraries(realsense_distance ${DEPENDENCIES})
③添加launch文件夹
- 在生成包文件夹下
mkdir launch & cd launch
- 新建新的文本文件,名字为YOUR_NAME.launch
<launch>
<node pkg="measure_distance" type="realsense_distance" name="realsense_distance" />
</launch>
④返回工作区,到catkin_ws目录下catkin build
⑤编译完成后开启launchroslaunch YOUR_PKG_NAME LAUNCH_NAME.launch
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)