29 #include <opencv2/core/version.hpp> 30 #if CV_MAJOR_VERSION == 2 31 #include <opencv2/contrib/contrib.hpp> 33 #include <opencv2/imgproc/imgproc.hpp> 38 #include <sensor_msgs/Image.h> 63 void imageCallback(
const sensor_msgs::ImageConstPtr& image_msg);
104 #if CV_MAJOR_VERSION == 3 124 color_lut_ = cv::Mat::zeros(1, NUM_GRAY_VALUES, CV_8UC3);
136 bool colormap_specified = priv_nh.
getParam(
"colormap", color_param);
137 if (colormap_specified)
149 if (priv_nh.
getParam(
"colors", color_param))
154 else if (!colormap_specified)
156 ROS_ERROR(
"Color transformation was not specified. Images will ");
157 ROS_ERROR(
"only be converted to their gray scale equivalents");
172 const sensor_msgs::ImageConstPtr& image_msg)
186 "Changing image colors is only supported for MONO8 images");
194 cv::Mat modified_image = cv::Mat::zeros(
195 original_image->image.rows,
196 original_image->image.cols,
201 original_image->image,
207 output->image = modified_image;
209 output->header = image_msg->header;
223 color_lut_.at<cv::Vec3b>(0, idx) = cv::Vec3b(idx, idx, idx);
241 ROS_ERROR(
"Colormap specification must be a two entry array with the");
243 ROS_ERROR(
"\t[\"colormap_name\", num_colors]");
248 std::string colormap_name;
250 int32_t colormap_idx;
262 ROS_ERROR(
"First colormap parameter must be a string");
270 ROS_ERROR(
"Second colormap parameter must be an integer");
277 colormap_name =
static_cast<std::string
>(colormap_param);
278 num_entries =
static_cast<int32_t
>(num_entries_param);
283 if (success && (num_entries <= 1))
285 ROS_ERROR(
"Must use at least two colors from the colormap");
293 if (num_entries > NUM_GRAY_VALUES)
295 ROS_ERROR(
"Number of colormap entries was greater");
304 std::map<std::string, int32_t>::iterator iter =
312 ROS_ERROR(
"Unknown colormap: %s requested", colormap_name.c_str());
324 cv::applyColorMap(original_colors,
color_lut_, colormap_idx);
340 for (int32_t replace_idx = 0; replace_idx < lut_size; replace_idx++)
342 int32_t lookup_idx = replace_idx % num_entries;
343 lookup_idx = lookup_idx * lut_size / num_entries;
344 color_lut_.at<cv::Vec3b>(0,
static_cast<uint8_t
>(replace_idx)) =
345 original_colors.at<cv::Vec3b>(0,
static_cast<uint8_t
>(lookup_idx));
357 ROS_ERROR(
"LUT must be an array, but type was: %s",
370 for (uint32_t lut_idx = 0; success && (lut_idx < param.
size()); lut_idx++)
377 ROS_ERROR(
"LUT entries must be two entry arrays with the following");
380 ROS_ERROR(
"but for entry %d a %s was given",
386 if (lut_row.
size() != 2)
388 ROS_ERROR(
"LUT entries must be two entry arrays with the following");
391 ROS_ERROR(
"but entry %d had an array with only %d values",
392 lut_idx, lut_row.
size());
402 ROS_ERROR(
"Color to replace must be an integer, but a %s was given for",
410 int32_t gray_index =
static_cast<int32_t
>(map_key);
411 if ((gray_index >= NUM_GRAY_VALUES) || (gray_index < 0))
413 ROS_ERROR(
"Gray scale value for LUT entry %d was %d, but must be",
414 lut_idx, gray_index);
415 ROS_ERROR(
"between 0 and %d", NUM_GRAY_VALUES - 1);
421 uint8_t color_index =
static_cast<uint8_t
>(gray_index);
428 ROS_ERROR(
"RGB entries must be three entry arrays of integers, but");
429 ROS_ERROR(
"LUT row %d was a %s", lut_idx,
435 if (rgb_values.
size() != 3)
437 ROS_ERROR(
"RGB entries must be three entry arrays of integers, but");
438 ROS_ERROR(
"LUT row %d had %d entries", lut_idx, rgb_values.
size());
445 ROS_ERROR(
"RGB entries must be three entry arrays of integers, but");
446 ROS_ERROR(
"LUT row %d had a red value with type %s", lut_idx,
454 ROS_ERROR(
"RGB entries must be three entry arrays of integers, but");
455 ROS_ERROR(
"LUT row %d had a green value with type %s", lut_idx,
463 ROS_ERROR(
"RGB entries must be three entry arrays of integers, but");
464 ROS_ERROR(
"LUT row %d had a blue value with type %s", lut_idx,
471 int32_t original_red =
static_cast<int32_t
>(rgb_values[0]);
472 if ((original_red > MAX_RGB_VALUE) || (original_red < 0))
474 ROS_ERROR(
"Red value on row %d was %d, and must be between 0 and %d",
475 lut_idx, original_red, MAX_RGB_VALUE);
479 int32_t original_green =
static_cast<int32_t
>(rgb_values[1]);
480 if ((original_green > MAX_RGB_VALUE) || (original_green < 0))
482 ROS_ERROR(
"Green value on row %d was %d, and must be between 0 and %d",
483 lut_idx, original_green, MAX_RGB_VALUE);
487 int32_t original_blue =
static_cast<int32_t
>(rgb_values[2]);
488 if ((original_blue > MAX_RGB_VALUE) || (original_blue < 0))
490 ROS_ERROR(
"Blue value on row %d was %d, and must be between 0 and %d",
491 lut_idx, original_blue, MAX_RGB_VALUE);
497 uint8_t red =
static_cast<uint8_t
>(original_red);
498 uint8_t green =
static_cast<uint8_t
>(original_green);
499 uint8_t blue =
static_cast<uint8_t
>(original_blue);
502 cv::Vec3b rgb_entry(red, green, blue);
503 temp_lut.at<cv::Vec3b>(0, color_index) = rgb_entry;
517 std::string type_str =
"unknown";
521 type_str =
"invalid";
525 type_str =
"boolean";
541 type_str =
"date time";
545 type_str =
"base 64";
557 ROS_ERROR(
"Unknown XML RPC value type encountered");
558 type_str =
"unknown";
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
const int32_t NUM_GRAY_VALUES
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
image_transport::Publisher image_pub_
void replaceColors(const cv::Mat &original_image, const cv::Mat &lut, cv::Mat &modified_image)
void readColormap(const XmlRpc::XmlRpcValue ¶m)
void imageCallback(const sensor_msgs::ImageConstPtr &image_msg)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
Type const & getType() const
image_transport::Subscriber image_sub_
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
std::map< std::string, int32_t > colormap_names_
void publish(const sensor_msgs::Image &message) const
const int32_t MAX_RGB_VALUE
SWRI_NODELET_EXPORT_CLASS(swri_image_util, DrawPolygonNodelet)
#define ROS_ERROR_THROTTLE(period,...)
ros::NodeHandle & getNodeHandle() const
void readUserLut(const XmlRpc::XmlRpcValue ¶m)
bool getParam(const std::string &key, std::string &s) const
std::string getValueTypeString(const XmlRpc::XmlRpcValue &value)