fuse_images.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Kentaro Wada and JSK Lab
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/o2r other materials provided
16  * with the distribution.
17  * * Neither the name of Kentaro Wada and JSK Lab nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #ifndef JSK_PCL_ROS_FUSE_DEPTH_IMAGES_H_
36 #define JSK_PCL_ROS_FUSE_DEPTH_IMAGES_H_
37 
38 #include <vector>
39 
46 #include <sensor_msgs/Image.h>
48 
49 namespace jsk_pcl_ros
50 {
52  {
53  public:
54  FuseImages(const std::string& name, const std::string& encoding):
55  DiagnosticNodelet(name), encoding_(encoding) {}
56  protected:
57  virtual void onInit();
58  virtual void subscribe();
59  virtual void unsubscribe();
60  virtual bool validateInput(const sensor_msgs::Image::ConstPtr& in,
61  const int height_expected, const int width_expected, std::vector<cv::Mat>& inputs);
62 
64 
67  bool averaging_;
68  std::string encoding_;
69 
73 
75  std::vector<boost::shared_ptr<message_filters::Subscriber<sensor_msgs::Image> > > filters_;
76 
81  sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
82  sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> > > async_;
84  sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
85  sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> > > sync_;
86 
91  inline void
92  input_callback (const sensor_msgs::Image::ConstPtr &input)
93  {
94  sensor_msgs::Image imgmsg;
95  imgmsg.header.stamp = input->header.stamp;
96  nf_.add(boost::make_shared<sensor_msgs::Image> (imgmsg));
97  }
98 
99  virtual void inputCb(const sensor_msgs::Image::ConstPtr &in1, const sensor_msgs::Image::ConstPtr &in2,
100  const sensor_msgs::Image::ConstPtr &in3, const sensor_msgs::Image::ConstPtr &in4,
101  const sensor_msgs::Image::ConstPtr &in5, const sensor_msgs::Image::ConstPtr &in6,
102  const sensor_msgs::Image::ConstPtr &in7, const sensor_msgs::Image::ConstPtr &in8);
103 
104  virtual cv::Mat fuseInputs(std::vector<cv::Mat> inputs) {};
105  private:
106  };
107 
109  {
110  public:
111  FuseDepthImages(): FuseImages("FuseDepthImages", sensor_msgs::image_encodings::TYPE_32FC1) {};
112  protected:
113  virtual cv::Mat fuseInputs(std::vector<cv::Mat> inputs);
114  };
115 
116  class FuseRGBImages: public FuseImages
117  {
118  public:
119  FuseRGBImages(): FuseImages("FuseRGBImages", sensor_msgs::image_encodings::RGB8) {};
120  protected:
121  virtual cv::Mat fuseInputs(std::vector<cv::Mat> inputs);
122  };
123 
124 } // namespace jsk_pcl_ros
125 
126 #endif // JSK_PCL_ROS_FUSE_DEPTH_IMAGES_H_
boost::shared_ptr< message_filters::Synchronizer< message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image > > > async_
Synchronizer.
Definition: fuse_images.h:82
virtual void subscribe()
Definition: fuse_images.cpp:55
virtual bool validateInput(const sensor_msgs::Image::ConstPtr &in, const int height_expected, const int width_expected, std::vector< cv::Mat > &inputs)
const std::string RGB8
virtual void unsubscribe()
imgmsg
FuseImages(const std::string &name, const std::string &encoding)
Definition: fuse_images.h:54
void input_callback(const sensor_msgs::Image::ConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
Definition: fuse_images.h:92
DiagnosticNodelet(const std::string &name)
const std::string TYPE_32FC1
virtual cv::Mat fuseInputs(std::vector< cv::Mat > inputs)
Definition: fuse_images.h:104
virtual void inputCb(const sensor_msgs::Image::ConstPtr &in1, const sensor_msgs::Image::ConstPtr &in2, const sensor_msgs::Image::ConstPtr &in3, const sensor_msgs::Image::ConstPtr &in4, const sensor_msgs::Image::ConstPtr &in5, const sensor_msgs::Image::ConstPtr &in6, const sensor_msgs::Image::ConstPtr &in7, const sensor_msgs::Image::ConstPtr &in8)
virtual void onInit()
Definition: fuse_images.cpp:48
std::vector< boost::shared_ptr< message_filters::Subscriber< sensor_msgs::Image > > > filters_
A vector of message filters.
Definition: fuse_images.h:75
void add(const MConstPtr &msg)
message_filters::PassThrough< sensor_msgs::Image > nf_
Null passthrough filter, used for pushing empty elements in the synchronizer.
Definition: fuse_images.h:72
boost::shared_ptr< message_filters::Synchronizer< message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image > > > sync_
Definition: fuse_images.h:85
ros::Publisher pub_out_
Definition: fuse_images.h:63


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46