RGBD_Image.msg
- 包含2个
sensor_msgs/Image，分别存储 rgb，depth - 添加1个
std_msgs/Header，存储图像时间戳，seq；sensor Image 自带的 header 就不用赋值了。
# use this header as both RGB & Depth header
std_msgs/Header header
# not use rgb/depth own header(in code)
sensor_msgs/Image rgb
sensor_msgs/Image depth
stereo_rgbd_puber.cpp
- opencv 读入
cv::Matrgb，depth - cv_bridge 将
cv::Mat转化为sensor_msgs::Image - 发布
注意：
-
n.advertise<stereo::RGBD_Image>指定为自定义的 msg - cv_bridge 图像转化编码：
RGB 编码bgr8，depth_8 编码mono8，depth_16 编码mono16
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/Image.h>
#include <stereo/RGBD_Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
int main(int argc, char **argv) {
ros::init(argc, argv, "stereo_rgbd_puber");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<stereo::RGBD_Image>("/stereo/rgbd/image", 1000); // Note: change msg type!
ros::Rate loop_rate(10); // 10fps
stereo::RGBD_Image rgbd_msg;
// msg source
cv::Mat rgb = cv::imread("/home/nvidia/wali_ws/src/stereo/img/rgb.jpg", cv::IMREAD_COLOR);
cv::Mat depth = cv::imread("/home/nvidia/wali_ws/src/stereo/img/depth.png", cv::IMREAD_ANYDEPTH);
// header
std_msgs::Header header; // not necessary
header.frame_id = "/stereo/rgbd/image"; // detailed header info
header.stamp = ros::Time::now(); // time stamp
unsigned int count = 0;
while (ros::ok()) {
header.seq = count;
rgbd_msg.header = header;
cv_bridge::CvImage(std_msgs::Header(), "bgr8", rgb).toImageMsg(rgbd_msg.rgb);
cv_bridge::CvImage(std_msgs::Header(), "mono16", depth).toImageMsg(rgbd_msg.depth);
// though default sensor_msgs/Image use uint8[] data, we use "mono16" can get 16bit data
ROS_INFO("send RGBD %d", count);
chatter_pub.publish(rgbd_msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
stereo_rgbd_suber.cpp 订阅 RGBD_Image 并显示
-
cv::imshow()显示 depth_16 要先转化为 depth_8UC3
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/Image.h>
#include <stereo/RGBD_Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
cv_bridge::CvImagePtr cv_ptr_rgb, cv_ptr_depth;
cv::Mat rgb, depth;
void chatterCallback(const stereo::RGBD_Image::ConstPtr &msg) {
ROS_INFO("get RGBD %d", msg->header.seq);
cv_ptr_rgb = cv_bridge::toCvCopy(msg->rgb, "bgr8");
cv_ptr_depth = cv_bridge::toCvCopy(msg->depth, "mono16"); // 16bits depth
rgb = cv_ptr_rgb->image;
depth = cv_ptr_depth->image;
cv::imshow("rgb", rgb);
// cvt 1C depth to 3C for imshow
cv::Mat depth_show(depth.rows, depth.cols, CV_8UC3);
for (int h = 0; h < depth.rows; ++h) {
for (int w = 0; w < depth.cols; ++w) { // ushort for 16 bit
uchar val = (uchar) (depth.at<ushort>(h, w) / 256); // 16->8
depth_show.at<cv::Vec3b>(h, w) = {val, val, val};
}
}
cv::imshow("depth", depth_show); // default 3 channels
cv::waitKey(30);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "stereo_rgbd_suber");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/stereo/rgbd/image", 1000, chatterCallback);
ros::spin();
return 0;
}
send, get, and show
CMakeLists.txt
- 添加 msg 生成
- 添加 opencv 库
cmake_minimum_required(VERSION 2.8.3)
project(stereo)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
sensor_msgs
message_generation
cv_bridge
)
find_package(OpenCV REQUIRED)
add_message_files(
FILES
RGBD_Image.msg
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs # use Image
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES stereo
CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
## 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(stereo_rgbd_puber src/stereo_rgbd_puber.cpp)
add_executable(stereo_rgbd_suber src/stereo_rgbd_suber.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# rgbd_puber
add_dependencies(stereo_rgbd_puber
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(stereo_rgbd_puber
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
# rgbd_suber
add_dependencies(stereo_rgbd_suber
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(stereo_rgbd_suber
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>stereo</name>
<version>0.0.0</version>
<description>The stereo package</description>
<maintainer email="nvidia@todo.todo">nvidia</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
网友评论