sparse_image_decoder.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 SparseImageDecoder: public nodelet::Nodelet
00018 {
00019   image_transport::Publisher _img_pub;
00020   ros::Subscriber _spr_img_sub;
00021 
00022   sensor_msgs::ImagePtr _img_ptr;
00023 
00024   boost::shared_ptr<image_transport::ImageTransport> _it;
00025   ros::NodeHandle _nh;
00026   int _subscriber_count;
00027 
00028   void imageCallback(const jsk_recognition_msgs::SparseImageConstPtr& msg){
00029     do_work(msg, msg->header.frame_id);
00030   }
00031 
00032   void do_work(const jsk_recognition_msgs::SparseImageConstPtr& msg, const std::string input_frame_from_msg){
00033     try {
00034 
00035       _img_ptr->header.stamp = msg->header.stamp;
00036       _img_ptr->header.frame_id = input_frame_from_msg;
00037       _img_ptr->width  = msg->width;
00038       _img_ptr->height = msg->height;
00039       _img_ptr->step = msg->width;
00040       _img_ptr->encoding = enc::MONO8;
00041       if (!_img_ptr->data.empty()) _img_ptr->data.clear();
00042 
00043       // check if uint16 array needed
00044       bool useData32 = false;
00045       int length = msg->data16.size();
00046       if (length <= 0) {
00047         useData32 = true;
00048         length = msg->data32.size();
00049         NODELET_DEBUG("use data32 array");
00050       }
00051       _img_ptr->data.resize(_img_ptr->width * _img_ptr->height);
00052       // decode sparse image -> image
00053       for (int i = 0; i < length; ++i){
00054         uint16_t x, y;
00055         if (useData32) {
00056           uint32_t pos = msg->data32[i];
00057           x = (uint16_t)(pos >> 16);
00058           y = (uint16_t)pos;
00059         } else {
00060           uint16_t pos = msg->data16[i];
00061           x = (uint16_t)(pos >> 8);
00062           y = (uint16_t)(pos & (uint8_t)-1);
00063         }
00064         _img_ptr->data[y * _img_ptr->width + x] = 255;
00065       }
00066 
00067       // publish image message
00068       _img_pub.publish(*_img_ptr);
00069     } // end of try
00070     catch (...) {
00071       NODELET_ERROR("making sparse image error");
00072     }
00073   } // end of do_work function
00074 
00075   void subscribe() {
00076     NODELET_DEBUG("Subscribing to image topic.");
00077     _spr_img_sub = _nh.subscribe("sparse_image", 3, &SparseImageDecoder::imageCallback, this);
00078     ros::V_string names = boost::assign::list_of("sparse_image");
00079     jsk_topic_tools::warnNoRemap(names);
00080   }
00081 
00082   void unsubscribe() {
00083     NODELET_DEBUG("Unsubscribing from image topic.");
00084     _spr_img_sub.shutdown();
00085   }
00086 
00087   void connectCb(const image_transport::SingleSubscriberPublisher& ssp) {
00088     if (_subscriber_count++ == 0) {
00089       subscribe();
00090     }
00091   }
00092 
00093   void disconnectCb(const image_transport::SingleSubscriberPublisher&) {
00094     _subscriber_count--;
00095     if (_subscriber_count == 0) {
00096       unsubscribe();
00097     }
00098   }
00099 
00100 public:
00101   void onInit() {
00102     _nh = getNodeHandle();
00103     _img_ptr.reset(new sensor_msgs::Image());
00104     _it.reset(new image_transport::ImageTransport(_nh));
00105     _subscriber_count = 0;
00106     image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&SparseImageDecoder::connectCb, this, _1);
00107     image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&SparseImageDecoder::disconnectCb, this, _1);
00108     _img_pub = image_transport::ImageTransport(ros::NodeHandle(_nh, "sparse")).advertise("image_decoded", 1, connect_cb, disconnect_cb);
00109   } // end of onInit function
00110 }; // end of SparseImageDecoder class definition
00111 } // end of jsk_perception namespace
00112 
00113 #include <pluginlib/class_list_macros.h>
00114 PLUGINLIB_EXPORT_CLASS(jsk_perception::SparseImageDecoder, nodelet::Nodelet);


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