Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros_utils/polygon_array_area_likelihood.h"
00039 #include "jsk_recognition_utils/tf_listener_singleton.h"
00040 #include "jsk_recognition_utils/geo_util.h"
00041 #include "jsk_recognition_utils/pcl_conversion_util.h"
00042
00043 namespace jsk_pcl_ros_utils
00044 {
00045 void PolygonArrayAreaLikelihood::onInit()
00046 {
00047 DiagnosticNodelet::onInit();
00048 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00050 boost::bind (&PolygonArrayAreaLikelihood::configCallback, this, _1, _2);
00051 srv_->setCallback (f);
00052 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00053 onInitPostProcess();
00054 }
00055
00056 void PolygonArrayAreaLikelihood::subscribe()
00057 {
00058 sub_ = pnh_->subscribe(
00059 "input", 1, &PolygonArrayAreaLikelihood::likelihood, this);
00060 }
00061
00062 void PolygonArrayAreaLikelihood::unsubscribe()
00063 {
00064 sub_.shutdown();
00065 }
00066
00067 void PolygonArrayAreaLikelihood::likelihood(
00068 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
00069 {
00070 boost::mutex::scoped_lock lock(mutex_);
00071 vital_checker_->poke();
00072 jsk_recognition_msgs::PolygonArray new_msg(*msg);
00073
00074 double min_area = DBL_MAX;
00075 double max_area = - DBL_MAX;
00076 std::vector<double> areas;
00077 for (size_t i = 0; i < msg->polygons.size(); i++) {
00078 jsk_recognition_utils::Polygon::Ptr polygon
00079 = jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
00080 double area = polygon->area();
00081 min_area = std::min(area, min_area);
00082 max_area = std::max(area, max_area);
00083 areas.push_back(area);
00084 }
00085
00086
00087 for (size_t i = 0; i < areas.size(); i++) {
00088 double diff = areas[i] - area_;
00089 double likelihood = 1 / (1 + (diff * diff));
00090 if (msg->likelihood.size() == 0) {
00091 new_msg.likelihood.push_back(likelihood);
00092 }
00093 else {
00094 new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
00095 }
00096 }
00097 pub_.publish(new_msg);
00098 }
00099
00100 void PolygonArrayAreaLikelihood::configCallback(
00101 Config& config, uint32_t level)
00102 {
00103 boost::mutex::scoped_lock lock(mutex_);
00104 area_ = config.area;
00105 }
00106
00107
00108 }
00109
00110 #include <pluginlib/class_list_macros.h>
00111 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonArrayAreaLikelihood,
00112 nodelet::Nodelet);