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