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
00037
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/common/io.h>
00040 #include "pcl_ros/surface/convex_hull.h"
00041 #include <geometry_msgs/PolygonStamped.h>
00042
00044 void
00045 pcl_ros::ConvexHull2D::onInit ()
00046 {
00047 ros::NodeHandle pnh = getMTPrivateNodeHandle ();
00048 pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_);
00049 pub_plane_ = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_);
00050
00051
00052 pnh.getParam ("use_indices", use_indices_);
00053
00054
00055 if (use_indices_)
00056 {
00057
00058 sub_input_filter_.subscribe (pnh, "input", 1);
00059
00060 sub_indices_filter_.subscribe (pnh, "indices", 1);
00061
00062 if (approximate_sync_)
00063 {
00064 sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
00065
00066 sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00067 sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
00068 }
00069 else
00070 {
00071 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
00072
00073 sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00074 sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
00075 }
00076 }
00077 else
00078
00079 sub_input_ = pnh.subscribe<PointCloud> ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00080
00081 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00082 " - use_indices : %s",
00083 getName ().c_str (),
00084 (use_indices_) ? "true" : "false");
00085 }
00086
00088 void
00089 pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
00090 const PointIndicesConstPtr &indices)
00091 {
00092
00093 if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
00094 return;
00095
00096 PointCloud output;
00097
00098
00099 if (!isValid (cloud))
00100 {
00101 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00102
00103 output.header = cloud->header;
00104 pub_output_.publish (output.makeShared ());
00105 return;
00106 }
00107
00108 if (indices && !isValid (indices, "indices"))
00109 {
00110 NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00111
00112 output.header = cloud->header;
00113 pub_output_.publish (output.makeShared ());
00114 return;
00115 }
00116
00118 if (indices)
00119 NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
00120 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00121 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00122 getName ().c_str (),
00123 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
00124 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
00125 else
00126 NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
00127
00128
00129 IndicesPtr indices_ptr;
00130 if (indices)
00131 indices_ptr.reset (new std::vector<int> (indices->indices));
00132
00133 impl_.setInputCloud (cloud);
00134 impl_.setIndices (indices_ptr);
00135
00136
00137 impl_.reconstruct (output);
00138
00139
00140 if (output.points.size () >= 3)
00141 {
00142 geometry_msgs::PolygonStamped poly;
00143 poly.header = fromPCL(output.header);
00144 poly.polygon.points.resize (output.points.size ());
00145
00146 pcl::Vector4fMap O = output.points[1].getVector4fMap ();
00147 pcl::Vector4fMap B = output.points[0].getVector4fMap ();
00148 pcl::Vector4fMap A = output.points[2].getVector4fMap ();
00149
00150 Eigen::Vector4f OA = A - O;
00151 Eigen::Vector4f OB = B - O;
00152 Eigen::Vector4f N = OA.cross3 (OB);
00153 double theta = N.dot (O);
00154 bool reversed = false;
00155 if (theta < (M_PI / 2.0))
00156 reversed = true;
00157 for (size_t i = 0; i < output.points.size (); ++i)
00158 {
00159 if (reversed)
00160 {
00161 size_t j = output.points.size () - i - 1;
00162 poly.polygon.points[i].x = output.points[j].x;
00163 poly.polygon.points[i].y = output.points[j].y;
00164 poly.polygon.points[i].z = output.points[j].z;
00165 }
00166 else
00167 {
00168 poly.polygon.points[i].x = output.points[i].x;
00169 poly.polygon.points[i].y = output.points[i].y;
00170 poly.polygon.points[i].z = output.points[i].z;
00171 }
00172 }
00173 pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
00174 }
00175
00176 output.header = cloud->header;
00177 pub_output_.publish (output.makeShared ());
00178 }
00179
00180 typedef pcl_ros::ConvexHull2D ConvexHull2D;
00181 PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet)
00182