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_foot_angle_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 #include <jsk_topic_tools/rosparam_utils.h>
00043
00044 namespace jsk_pcl_ros_utils
00045 {
00046 void PolygonArrayFootAngleLikelihood::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 if (!pnh_->getParam("target_frame_id", target_frame_id_)) {
00050 ROS_ERROR("You need to specify ~target_frame_id");
00051 return;
00052 }
00053 pnh_->param("tf_queue_size", tf_queue_size_, 10);
00054 std::vector<double> axis(3);
00055 if (!jsk_topic_tools::readVectorParameter(*pnh_, "axis", axis)) {
00056 axis[0] = 1;
00057 axis[1] = 0;
00058 axis[2] = 0;
00059 }
00060 axis_[0] = axis[0];
00061 axis_[1] = axis[1];
00062 axis_[2] = axis[2];
00063 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00064 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00065
00066 onInitPostProcess();
00067 }
00068
00069 void PolygonArrayFootAngleLikelihood::subscribe()
00070 {
00071 sub_.subscribe(*pnh_, "input", 10);
00072 tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::PolygonArray>(
00073 sub_,
00074 *tf_listener_,
00075 target_frame_id_,
00076 tf_queue_size_));
00077 tf_filter_->registerCallback(
00078 boost::bind(&PolygonArrayFootAngleLikelihood::likelihood, this, _1));
00079 }
00080
00081 void PolygonArrayFootAngleLikelihood::unsubscribe()
00082 {
00083 sub_.unsubscribe();
00084 }
00085
00086 void PolygonArrayFootAngleLikelihood::likelihood(
00087 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
00088 {
00089 boost::mutex::scoped_lock lock(mutex_);
00090 vital_checker_->poke();
00091 jsk_recognition_msgs::PolygonArray new_msg(*msg);
00092
00093 try
00094 {
00095
00096
00097
00098 tf::StampedTransform transform;
00099 tf_listener_->lookupTransform(
00100 target_frame_id_, msg->header.frame_id, msg->header.stamp, transform);
00101 Eigen::Affine3f pose;
00102 tf::transformTFToEigen(transform, pose);
00103
00104 Eigen::Vector3f reference_axis = pose.rotation() * axis_;
00105
00106 std::vector<double> distances;
00107 for (size_t i = 0; i < msg->polygons.size(); i++) {
00108 jsk_recognition_utils::Polygon::Ptr polygon
00109 = jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
00110 double distance;
00111 Eigen::Vector3f foot = polygon->nearestPoint(pose.translation(), distance);
00112 Eigen::Vector3f foot_dir = (foot - pose.translation()).normalized();
00113 double ang_distance = std::abs(reference_axis.dot(foot_dir));
00114 distances.push_back(ang_distance);
00115 }
00116
00117
00118 for (size_t i = 0; i < distances.size(); i++) {
00119
00120
00121 double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1));
00122
00123 if (msg->likelihood.size() == 0) {
00124 new_msg.likelihood.push_back(likelihood);
00125 }
00126 else {
00127 new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
00128 }
00129 }
00130 pub_.publish(new_msg);
00131 }
00132 catch (...)
00133 {
00134 NODELET_ERROR("Unknown transform error");
00135 }
00136
00137 }
00138 }
00139
00140 #include <pluginlib/class_list_macros.h>
00141 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonArrayFootAngleLikelihood,
00142 nodelet::Nodelet);