polygon_array_distance_likelihood_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 
00038 #include "jsk_pcl_ros/polygon_array_distance_likelihood.h"
00039 #include "jsk_pcl_ros/tf_listener_singleton.h"
00040 #include "jsk_pcl_ros/geo_util.h"
00041 #include "jsk_pcl_ros/pcl_conversion_util.h"
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   void PolygonArrayDistanceLikelihood::onInit()
00046   {
00047     DiagnosticNodelet::onInit();
00048     if (!pnh_->getParam("target_frame_id", target_frame_id_)) {
00049       JSK_ROS_ERROR("You need to specify ~target_frame_id");
00050       return;
00051     }
00052     pnh_->param("tf_queue_size", tf_queue_size_, 10);
00053     tf_listener_ = TfListenerSingleton::getInstance();
00054     pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00055   }
00056 
00057   void PolygonArrayDistanceLikelihood::subscribe()
00058   {
00059     sub_.subscribe(*pnh_, "input", 10);
00060     tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::PolygonArray>(
00061                        sub_,
00062                        *tf_listener_,
00063                        target_frame_id_,
00064                        tf_queue_size_));
00065     tf_filter_->registerCallback(
00066       boost::bind(&PolygonArrayDistanceLikelihood::likelihood, this, _1));
00067   }
00068 
00069   void PolygonArrayDistanceLikelihood::unsubscribe()
00070   {
00071     sub_.unsubscribe();
00072   }
00073 
00074   void PolygonArrayDistanceLikelihood::likelihood(
00075     const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
00076   {
00077     boost::mutex::scoped_lock lock(mutex_);
00078     jsk_recognition_msgs::PolygonArray new_msg(*msg);
00079 
00080     try
00081     {
00082       // Resolve tf
00083       // ConstPtrmpute position of target_frame_id
00084       // respected from msg->header.frame_id
00085       tf::StampedTransform transform;
00086       tf_listener_->lookupTransform(
00087         msg->header.frame_id, target_frame_id_, msg->header.stamp, transform);
00088       Eigen::Affine3f pose;
00089       tf::transformTFToEigen(transform, pose);
00090       Eigen::Vector3f p(pose.translation());
00091 
00092       double min_distance = DBL_MAX;
00093       double max_distance = - DBL_MAX;
00094       std::vector<double> distances; 
00095       for (size_t i = 0; i < msg->polygons.size(); i++) {
00096         Polygon::Ptr polygon = Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
00097         double distance;
00098         polygon->nearestPoint(p, distance);
00099         min_distance = std::min(distance, min_distance);
00100         max_distance = std::max(distance, max_distance);
00101         distances.push_back(distance);
00102       }
00103 
00104       // Normalization
00105       for (size_t i = 0; i < distances.size(); i++) {
00106         // double normalized_distance
00107         //   = (distances[i] - min_distance) / (max_distance - min_distance);
00108         double likelihood = 1 / (1 + distances[i] * distances[i]);
00109         
00110         if (msg->likelihood.size() == 0) {
00111           new_msg.likelihood.push_back(likelihood);
00112         }
00113         else {
00114           new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
00115         }
00116       }
00117       pub_.publish(new_msg);
00118     }
00119     catch (...)
00120     {
00121       JSK_NODELET_ERROR("Unknown transform error");
00122     }
00123     
00124   }
00125 }
00126 
00127 #include <pluginlib/class_list_macros.h>
00128 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PolygonArrayDistanceLikelihood,
00129                         nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48