polygon_flipper_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, 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 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros_utils/polygon_flipper.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039 #include <algorithm>
00040 #include <iterator>
00041 
00042 namespace jsk_pcl_ros_utils
00043 {
00044   void PolygonFlipper::onInit()
00045   {
00046     DiagnosticNodelet::onInit();
00047     if (!pnh_->getParam("sensor_frame", sensor_frame_)) {
00048       NODELET_FATAL("no ~sensor_frame is specified");
00049       return;
00050     }
00051 
00052     pnh_->param<int>("queue_size", queue_size_, 100);
00053     pnh_->param<bool>("use_indices", use_indices_, true);
00054 
00055     tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00056     pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(
00057       *pnh_, "output/polygons", 1);
00058     pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00059       *pnh_, "output/coefficients", 1);
00060     if (use_indices_)
00061       pub_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
00062         *pnh_, "output/indices", 1);
00063 
00064     onInitPostProcess();
00065   }
00066 
00067   void PolygonFlipper::subscribe()
00068   {
00069     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00070     sub_polygons_.subscribe(*pnh_, "input/polygons", 1);
00071     sub_coefficients_.subscribe(*pnh_, "input/coefficients", 1);
00072     if (use_indices_) {
00073       sub_indices_.subscribe(*pnh_, "input/indices", 1);
00074       sync_->connectInput(sub_polygons_, sub_indices_, sub_coefficients_);
00075     } else {
00076       sub_polygons_.registerCallback(boost::bind(&PolygonFlipper::fillEmptyIndices, this, _1));
00077       sync_->connectInput(sub_polygons_, sub_indices_null_, sub_coefficients_);
00078     }
00079     sync_->registerCallback(boost::bind(&PolygonFlipper::flip, this, _1, _2, _3));
00080   }
00081 
00082   void PolygonFlipper::unsubscribe()
00083   {
00084     sub_polygons_.unsubscribe();
00085     sub_coefficients_.unsubscribe();
00086     if (use_indices_)
00087       sub_indices_.unsubscribe();
00088   }
00089 
00090   void PolygonFlipper::fillEmptyIndices(
00091     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons_msg)
00092   {
00093     jsk_recognition_msgs::ClusterPointIndices indices;
00094     indices.header.stamp = polygons_msg->header.stamp;
00095     indices.cluster_indices.resize(polygons_msg->polygons.size());
00096     sub_indices_null_.add(
00097       boost::make_shared<jsk_recognition_msgs::ClusterPointIndices>(indices));
00098   }
00099 
00100   void PolygonFlipper::flip(
00101     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons_msg,
00102     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg,
00103     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
00104   {
00105     vital_checker_->poke();
00106     if (polygons_msg->polygons.size() != coefficients_msg->coefficients.size()) {
00107       NODELET_ERROR("The size of polygons and coefficients are not same");
00108       return;
00109     }
00110     jsk_recognition_msgs::PolygonArray flipped_polygons = *polygons_msg;
00111     jsk_recognition_msgs::ModelCoefficientsArray flipped_coefficients;
00112     jsk_recognition_msgs::ClusterPointIndices flipped_indices;
00113     flipped_polygons.polygons.clear();
00114     flipped_coefficients.header = coefficients_msg->header;
00115     flipped_indices.header = indices_msg->header;
00116     try {
00117       for (size_t i = 0; i < polygons_msg->polygons.size(); i++) {
00118         geometry_msgs::PolygonStamped target_polygon = polygons_msg->polygons[i];
00119         PCLModelCoefficientMsg target_coefficients = coefficients_msg->coefficients[i];
00120         PCLIndicesMsg target_indices = indices_msg->cluster_indices[i];
00121         tf::StampedTransform tf_transform
00122           = jsk_recognition_utils::lookupTransformWithDuration(
00123             tf_listener_, target_coefficients.header.frame_id,
00124             sensor_frame_, target_coefficients.header.stamp,
00125             ros::Duration(1.0));
00126         Eigen::Affine3f sensor_transform;
00127         tf::transformTFToEigen(tf_transform, sensor_transform);
00128         {
00129           // poygons
00130           jsk_recognition_utils::ConvexPolygon convex = jsk_recognition_utils::ConvexPolygon::fromROSMsg(target_polygon.polygon);
00131           Eigen::Vector3f polygon_normal = convex.getNormal();
00132           if (polygon_normal.dot(Eigen::Vector3f(sensor_transform.translation()))
00133               < 0) {
00134             geometry_msgs::PolygonStamped flipped_polygon;
00135             std::reverse_copy(
00136               target_polygon.polygon.points.begin(),
00137               target_polygon.polygon.points.end(),
00138               std::back_inserter(flipped_polygon.polygon.points));
00139             flipped_polygon.header = target_polygon.header;
00140             flipped_polygons.polygons.push_back(flipped_polygon);
00141           }
00142           else {
00143             flipped_polygons.polygons.push_back(target_polygon);
00144           }
00145         }
00146         
00147         {
00148           // coefficients
00149           Eigen::Vector3f local_normal(target_coefficients.values[0],
00150                                        target_coefficients.values[1],
00151                                        target_coefficients.values[2]);
00152           if (local_normal.dot(Eigen::Vector3f(sensor_transform.translation()))
00153               < 0) {
00154             PCLModelCoefficientMsg the_flipped_coefficients;
00155             PCLIndicesMsg the_flipped_indices;
00156             for (size_t j = 0; j < target_coefficients.values.size(); j++) {
00157               the_flipped_coefficients.values.push_back(
00158                 - target_coefficients.values[j]);
00159             }
00160             std::reverse_copy(target_indices.indices.begin(), target_indices.indices.end(),
00161                               std::back_inserter(the_flipped_indices.indices));
00162             the_flipped_coefficients.header = target_coefficients.header;
00163             the_flipped_indices.header = target_indices.header;
00164             flipped_coefficients.coefficients.push_back(the_flipped_coefficients);
00165             //flipped_coefficients.coefficients.push_back(target_coefficients);
00166             flipped_indices.cluster_indices.push_back(the_flipped_indices);
00167             //flipped_indices.cluster_indices.push_back(target_indices);
00168           }
00169           else {
00170             // no need to flip
00171             flipped_coefficients.coefficients.push_back(target_coefficients);
00172             flipped_indices.cluster_indices.push_back(target_indices);
00173           }
00174         }
00175       }
00176       pub_polygons_.publish(flipped_polygons);
00177       pub_coefficients_.publish(flipped_coefficients);
00178       if (use_indices_)
00179         pub_indices_.publish(flipped_indices);
00180     }
00181     catch (tf2::TransformException& e) {
00182       NODELET_ERROR("Failed to lookup transformation: %s", e.what());
00183     }
00184   }
00185 }
00186 
00187 #include <pluginlib/class_list_macros.h>
00188 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonFlipper, nodelet::Nodelet);


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:40:52