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 #include "jsk_pcl_ros_utils/colorize_distance_from_plane.h"
00037 #include "jsk_recognition_utils/pcl_conversion_util.h"
00038
00039 namespace jsk_pcl_ros_utils
00040 {
00041 void ColorizeDistanceFromPlane::onInit()
00042 {
00043 ConnectionBasedNodelet::onInit();
00044
00046
00048 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00049
00051
00053
00054 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00055 dynamic_reconfigure::Server<Config>::CallbackType f =
00056 boost::bind (&ColorizeDistanceFromPlane::configCallback, this, _1, _2);
00057 srv_->setCallback (f);
00058 }
00059
00060 void ColorizeDistanceFromPlane::subscribe()
00061 {
00063
00065 sub_input_.subscribe(*pnh_, "input", 1);
00066 sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
00067 sub_polygons_.subscribe(*pnh_, "input_polygons", 1);
00068
00069 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00070 sync_->connectInput(sub_input_, sub_coefficients_, sub_polygons_);
00071 sync_->registerCallback(boost::bind(
00072 &ColorizeDistanceFromPlane::colorize,
00073 this, _1, _2, _3));
00074 }
00075
00076 void ColorizeDistanceFromPlane::unsubscribe()
00077 {
00078 sub_input_.unsubscribe();
00079 sub_coefficients_.unsubscribe();
00080 sub_polygons_.unsubscribe();
00081 }
00082
00083 double ColorizeDistanceFromPlane::distanceToConvexes(
00084 const PointT& p,
00085 const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00086 {
00087 Eigen::Vector3f v = p.getVector3fMap();
00088 double min_distance = DBL_MAX;
00089 for (size_t i = 0; i < convexes.size(); i++) {
00090 jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
00091 if (!only_projectable_ || convex->isProjectableInside(v)) {
00092 double d = convex->distanceToPoint(v);
00093 if (d < min_distance) {
00094 min_distance = d;
00095 }
00096 }
00097 }
00098 return min_distance;
00099 }
00100
00101 uint32_t ColorizeDistanceFromPlane::colorForDistance(const double d)
00102 {
00103 double ratio = 0.0;
00104 if (d > max_distance_) {
00105 ratio = 1.0;
00106 }
00107 else if (d < min_distance_) {
00108 ratio = 0.0;
00109 }
00110 else {
00111 ratio = fabs(min_distance_ - d) / (max_distance_ - min_distance_);
00112 }
00113 double r = ratio;
00114 double g = 0.0;
00115 double b = 1 - ratio;
00116 uint8_t ru, gu, bu;
00117 ru = (uint8_t)(r * 255);
00118 gu = (uint8_t)(g * 255);
00119 bu = (uint8_t)(b * 255);
00120 return ((uint32_t)ru<<16 | (uint32_t)gu<<8 | (uint32_t)bu);
00121 }
00122
00123 void ColorizeDistanceFromPlane::colorize(
00124 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00125 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
00126 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
00127 {
00128 boost::mutex::scoped_lock lock(mutex_);
00129 if (coefficients_msg->coefficients.size() == 0) {
00130 return;
00131 }
00132
00133 pcl::PointCloud<PointT>::Ptr cloud
00134 (new pcl::PointCloud<PointT>);
00135 pcl::fromROSMsg(*cloud_msg, *cloud);
00136 std::vector<pcl::ModelCoefficients::Ptr> coefficients
00137 = pcl_conversions::convertToPCLModelCoefficients(
00138 coefficients_msg->coefficients);
00139
00140
00141 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes;
00142 for (size_t i = 0; i < polygons->polygons.size(); i++) {
00143 if (polygons->polygons[i].polygon.points.size() > 0) {
00144 jsk_recognition_utils::ConvexPolygon convex =
00145 jsk_recognition_utils::ConvexPolygon::fromROSMsg(polygons->polygons[i].polygon);
00146 jsk_recognition_utils::ConvexPolygon::Ptr convex_ptr
00147 = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex);
00148 convexes.push_back(convex_ptr);
00149 }
00150 else {
00151 NODELET_ERROR_STREAM(__PRETTY_FUNCTION__ << ":: there is no points in the polygon");
00152 }
00153 }
00154
00155 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud
00156 (new pcl::PointCloud<pcl::PointXYZRGB>);
00157
00158 for (size_t i = 0; i < cloud->points.size(); i++) {
00159 PointT p = cloud->points[i];
00160 pcl::PointXYZRGB p_output;
00161 jsk_recognition_utils::pointFromXYZToXYZ<PointT, pcl::PointXYZRGB>(p, p_output);
00162 double d = distanceToConvexes(p, convexes);
00163 if (d != DBL_MAX) {
00164 uint32_t color = colorForDistance(d);
00165 p_output.rgb = *reinterpret_cast<float*>(&color);
00166 output_cloud->points.push_back(p_output);
00167 }
00168 }
00169
00170 sensor_msgs::PointCloud2 ros_output;
00171 pcl::toROSMsg(*output_cloud, ros_output);
00172 ros_output.header = cloud_msg->header;
00173 pub_.publish(ros_output);
00174 }
00175
00176 void ColorizeDistanceFromPlane::configCallback(Config &config, uint32_t level)
00177 {
00178 boost::mutex::scoped_lock lock(mutex_);
00179 if (config.max_distance < config.min_distance) {
00180 if (max_distance_ != config.max_distance) {
00181 config.min_distance = config.max_distance;
00182 }
00183 else if (min_distance_ != config.min_distance) {
00184 config.max_distance = config.min_distance;
00185 }
00186 }
00187 else {
00188 max_distance_ = config.max_distance;
00189 min_distance_ = config.min_distance;
00190 }
00191 }
00192
00193 }
00194
00195 #include <pluginlib/class_list_macros.h>
00196 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::ColorizeDistanceFromPlane,
00197 nodelet::Nodelet);