Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00036 #include <ros/ros.h>
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <opencv/cv.h>
00039 #include <opencv/highgui.h>
00040 #include <sensor_msgs/Image.h>
00041
00042 namespace {
00043 const std::string WINDOW_NAME = "Depth View";
00044 double min_range_;
00045 double max_range_;
00046
00047 void depthCb(const sensor_msgs::ImageConstPtr& image) {
00048
00049 cv_bridge::CvImagePtr bridge;
00050 try
00051 {
00052 bridge = cv_bridge::toCvCopy(image, "32FC1");
00053 }
00054 catch (cv_bridge::Exception& e)
00055 {
00056 ROS_ERROR("Failed to transform depth image.");
00057 return;
00058 }
00059
00060
00061 cv::Mat img(bridge->image.rows, bridge->image.cols, CV_8UC1);
00062 for(int i = 0; i < bridge->image.rows; i++)
00063 {
00064 float* Di = bridge->image.ptr<float>(i);
00065 char* Ii = img.ptr<char>(i);
00066 for(int j = 0; j < bridge->image.cols; j++)
00067 {
00068 Ii[j] = (char) (255*((Di[j]-min_range_)/(max_range_-min_range_)));
00069 }
00070 }
00071
00072
00073 cv::imshow(WINDOW_NAME, img);
00074 }
00075 }
00076
00077 int main(int argc, char* argv[]) {
00078 ros::init( argc, argv, "depth_viewer" );
00079 ros::NodeHandle n;
00080
00081 ros::NodeHandle nh("~");
00082 nh.param("min_range", min_range_, 0.5);
00083 nh.param("max_range", max_range_, 10.0);
00084
00085 cv::namedWindow(WINDOW_NAME);
00086 cv::startWindowThread();
00087
00088 ros::Subscriber sub = n.subscribe("/camera/depth/image", 3, &depthCb);
00089 ros::spin();
00090 }