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
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_perception/colorize_float_image.h"
00037 #include <boost/assign.hpp>
00038 #include <opencv2/imgproc/imgproc.hpp>
00039 #include <jsk_topic_tools/log_utils.h>
00040 #include <sensor_msgs/image_encodings.h>
00041
00042 namespace jsk_perception
00043 {
00044 void ColorizeFloatImage::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00048 pnh_->param("channel", channel_, 0);
00049 onInitPostProcess();
00050 }
00051
00052 void ColorizeFloatImage::subscribe()
00053 {
00054 sub_ = pnh_->subscribe("input", 1, &ColorizeFloatImage::colorize, this);
00055 ros::V_string names = boost::assign::list_of("~input");
00056 jsk_topic_tools::warnNoRemap(names);
00057 }
00058
00059 void ColorizeFloatImage::unsubscribe()
00060 {
00061 sub_.shutdown();
00062 }
00063
00064 void ColorizeFloatImage::colorize(const sensor_msgs::Image::ConstPtr& msg)
00065 {
00066 int numchannels = sensor_msgs::image_encodings::numChannels(msg->encoding);
00067 if (numchannels < channel_) {
00068 ROS_ERROR("Image Channel(%s) %d is less than parameter channel (%d)",
00069 msg->encoding.c_str(),
00070 sensor_msgs::image_encodings::numChannels(msg->encoding),
00071 channel_);
00072 }
00073 cv::Mat float_image = cv_bridge::toCvShare(msg)->image;
00074 cv::Mat color_image = cv::Mat(float_image.rows, float_image.cols, CV_8UC3);
00075
00076 if (channel_ != 0 || numchannels > 1) {
00077 std::vector<cv::Mat> planes;
00078 cv::split(float_image, planes);
00079 float_image = planes[channel_];
00080 }
00081
00082 double min_value;
00083 double max_value;
00084 cv::minMaxLoc(float_image, &min_value, &max_value);
00085
00086 for (size_t j = 0; j < float_image.rows; j++) {
00087 for (size_t i = 0; i < float_image.cols; i++) {
00088 float v = float_image.at<float>(j, i);
00089 if (std::isnan(v)) {
00090 color_image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
00091 }
00092 else {
00093 std_msgs::ColorRGBA c = jsk_topic_tools::heatColor((v - min_value) / (max_value - min_value));
00094 color_image.at<cv::Vec3b>(j, i) = cv::Vec3b(c.r * 255, c.g * 255, c.b * 255);
00095 }
00096 }
00097 }
00098 pub_.publish(cv_bridge::CvImage(msg->header,
00099 sensor_msgs::image_encodings::RGB8,
00100 color_image).toImageMsg());
00101 }
00102
00103 }
00104
00105 #include <pluginlib/class_list_macros.h>
00106 PLUGINLIB_EXPORT_CLASS (jsk_perception::ColorizeFloatImage, nodelet::Nodelet);