sparse_image_encoder.cpp
Go to the documentation of this file.
00001 #include <stdint.h>
00002 #include <boost/shared_ptr.hpp>
00003 #include <boost/assign.hpp>
00004 #include <vector>
00005 #include <iostream>
00006 
00007 #include <nodelet/nodelet.h>
00008 #include <jsk_topic_tools/log_utils.h>
00009 #include <image_transport/image_transport.h>
00010 #include <sensor_msgs/image_encodings.h>
00011 #include <jsk_recognition_msgs/SparseImage.h>
00012 
00013 
00014 namespace enc = sensor_msgs::image_encodings;
00015 
00016 namespace jsk_perception {
00017 class SparseImageEncoder: public nodelet::Nodelet
00018 {
00019   ros::Publisher _spr_img_pub;
00020   image_transport::Subscriber _img_sub;
00021 
00022   jsk_recognition_msgs::SparseImagePtr _spr_img_ptr;
00023 
00024   boost::shared_ptr<image_transport::ImageTransport> _it;
00025   ros::NodeHandle _nh;
00026   ros::NodeHandle _ln;
00027   int _subscriber_count;
00028   double _rate;
00029   bool _print_point_num;
00030 
00031   void imageCallback(const sensor_msgs::ImageConstPtr& msg){
00032     do_work(msg, msg->header.frame_id);
00033   }
00034 
00035   void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg){
00036     try {
00037       int channel = enc::numChannels(msg->encoding);
00038 
00039       // check if uint16 array needed
00040       bool useData32 = false;
00041       if (std::max(msg->width, msg->height) > 256){
00042         useData32 = true;
00043         NODELET_DEBUG("use data32 array");
00044       }
00045 
00046       _spr_img_ptr->header.stamp = msg->header.stamp;
00047       _spr_img_ptr->header.frame_id = input_frame_from_msg;
00048       _spr_img_ptr->width  = msg->width;
00049       _spr_img_ptr->height = msg->height;
00050       if (!_spr_img_ptr->data16.empty()) _spr_img_ptr->data16.clear();
00051       if (!_spr_img_ptr->data32.empty()) _spr_img_ptr->data32.clear();
00052 
00053       // make sparse image
00054       for (uint32_t y = 0; y < msg->height; ++y){
00055         for (uint32_t x = 0; x < msg->width; ++x){
00056           if(msg->data[x*channel+y*msg->step] > 125){
00057             if (useData32) _spr_img_ptr->data32.push_back( (x << 16) | y );
00058             else           _spr_img_ptr->data16.push_back( (x << 8)  | y );
00059           }
00060         }
00061       }
00062 
00063       // print number of point if enabled
00064       if (_print_point_num) {
00065         int size = 0;
00066         if (useData32) size = _spr_img_ptr->data32.size();
00067         else size = _spr_img_ptr->data16.size();
00068         NODELET_INFO("%d point encoded. encoded area per is %lf%%.", size, (100 * ((double) size)/(msg->height* msg->width)));
00069       }
00070 
00071       // publish sparse image message
00072       _spr_img_pub.publish(*_spr_img_ptr);
00073     } // end of try
00074     catch (...) {
00075       NODELET_ERROR("making sparse image error");
00076     }
00077 
00078     ros::Rate pubRate(_rate); // hz
00079     pubRate.sleep();
00080   } // end of do_work function
00081 
00082   void subscribe() {
00083     NODELET_DEBUG("Subscribing to image topic.");
00084     _img_sub = _it->subscribe("image", 3, &SparseImageEncoder::imageCallback, this);
00085     ros::V_string names = boost::assign::list_of("image");
00086     jsk_topic_tools::warnNoRemap(names);
00087   }
00088 
00089   void unsubscribe() {
00090     NODELET_DEBUG("Unsubscribing from image topic.");
00091     _img_sub.shutdown();
00092   }
00093 
00094   void connectCb(const ros::SingleSubscriberPublisher& ssp) {
00095     if (_subscriber_count++ == 0) {
00096       subscribe();
00097     }
00098   }
00099 
00100   void disconnectCb(const ros::SingleSubscriberPublisher&) {
00101     _subscriber_count--;
00102     if (_subscriber_count == 0) {
00103       unsubscribe();
00104     }
00105   }
00106 
00107 public:
00108   void onInit() {
00109     _nh = getNodeHandle();
00110     _ln = ros::NodeHandle("~");
00111     _it.reset(new image_transport::ImageTransport(_nh));
00112     _subscriber_count = 0;
00113     ros::SubscriberStatusCallback connect_cb    = boost::bind(&SparseImageEncoder::connectCb, this, _1);
00114     ros::SubscriberStatusCallback disconnect_cb = boost::bind(&SparseImageEncoder::disconnectCb, this, _1);
00115     _spr_img_pub = _nh.advertise<jsk_recognition_msgs::SparseImage>("sparse_image", 10, connect_cb, disconnect_cb);
00116     _spr_img_ptr = boost::make_shared<jsk_recognition_msgs::SparseImage>();
00117     _ln.param("rate", _rate, 3.0);
00118     _ln.param("print_point_num", _print_point_num, false);
00119   } // end of onInit function
00120 }; // end of SparseImageEncoder class definition
00121 } // end of jsk_perception namespace
00122 
00123 #include <pluginlib/class_list_macros.h>
00124 PLUGINLIB_EXPORT_CLASS(jsk_perception::SparseImageEncoder, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23