polygon_appender_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_pcl_ros_utils/polygon_appender.h"
00037 #include <pluginlib/class_list_macros.h>
00038 
00039 namespace jsk_pcl_ros_utils
00040 {
00041   void PolygonAppender::onInit()
00042   {
00043     ConnectionBasedNodelet::onInit();
00044     pub_polygon_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00045     pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
00046       "output_coefficients", 1);
00047     
00048     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy2> >(100);
00049     sync_->connectInput(sub_polygon0_, sub_coefficients0_,
00050                         sub_polygon1_, sub_coefficients1_);
00051     sync_->registerCallback(boost::bind(&PolygonAppender::callback2, this, _1, _2, _3, _4));
00052   }
00053 
00054   void PolygonAppender::subscribe()
00055   {
00056     sub_polygon0_.subscribe(*pnh_, "input0", 1);
00057     sub_polygon1_.subscribe(*pnh_, "input1", 1);
00058     sub_coefficients0_.subscribe(*pnh_, "input_coefficients0", 1);
00059     sub_coefficients1_.subscribe(*pnh_, "input_coefficients1", 1);
00060   }
00061 
00062   void PolygonAppender::unsubscribe()
00063   {
00064     sub_polygon0_.unsubscribe();
00065     sub_polygon1_.unsubscribe();
00066     sub_coefficients0_.unsubscribe();
00067     sub_coefficients1_.unsubscribe();
00068   }
00069   
00070   void PolygonAppender::callback2(
00071     const jsk_recognition_msgs::PolygonArray::ConstPtr& msg0,
00072     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients0,
00073     const jsk_recognition_msgs::PolygonArray::ConstPtr& msg1,
00074     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients1)
00075   {
00076     std::vector<jsk_recognition_msgs::PolygonArray::ConstPtr> arrays;
00077     arrays.push_back(msg0);
00078     arrays.push_back(msg1);
00079     std::vector<jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr> coefficients_array;
00080     coefficients_array.push_back(coefficients0);
00081     coefficients_array.push_back(coefficients1);
00082     appendAndPublish(arrays, coefficients_array);
00083   }
00084 
00085   void PolygonAppender::appendAndPublish(
00086     const std::vector<jsk_recognition_msgs::PolygonArray::ConstPtr>& arrays,
00087     const std::vector<jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr>& coefficients_array)
00088   {
00089     if (arrays.size() == 0) {
00090       NODELET_ERROR("there is not enough polygons");
00091       return;
00092     }
00093     if (coefficients_array.size() == 0) {
00094       NODELET_ERROR("there is not enough coefficients");
00095       return;
00096     }
00097     if (arrays.size() != coefficients_array.size()) {
00098       NODELET_ERROR("polygons and coefficients are not the same length");
00099       return;
00100     }
00101     jsk_recognition_msgs::PolygonArray new_array;
00102     new_array.header = arrays[0]->header;
00103     for (size_t i = 0; i < arrays.size(); i++) {
00104       jsk_recognition_msgs::PolygonArray::ConstPtr array = arrays[i];
00105       for (size_t j = 0; j < array->polygons.size(); j++) {
00106         geometry_msgs::PolygonStamped polygon = array->polygons[j];
00107         new_array.polygons.push_back(polygon);
00108       }
00109     }
00110     pub_polygon_.publish(new_array);
00111 
00112     jsk_recognition_msgs::ModelCoefficientsArray coefficients_new_array;
00113     coefficients_new_array.header = coefficients_array[0]->header;
00114     for (size_t i = 0; i < coefficients_array.size(); i++) {
00115       jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr array = coefficients_array[i];
00116       for (size_t j = 0; j < array->coefficients.size(); j++) {
00117         coefficients_new_array.coefficients.push_back(array->coefficients[j]);
00118       }
00119     }
00120     pub_coefficients_.publish(coefficients_new_array);
00121   }
00122 }
00123 
00124 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonAppender, nodelet::Nodelet);
00125 


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:05