static_polygon_array_publisher_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <jsk_pcl_ros/static_polygon_array_publisher.h>
00036 #include <pluginlib/class_list_macros.h>
00037 #include <jsk_topic_tools/rosparam_utils.h>
00038 
00039 namespace jsk_pcl_ros
00040 {
00041 
00042   void StaticPolygonArrayPublisher::onInit()
00043   {
00044     ConnectionBasedNodelet::onInit();
00045     pnh_->param("use_periodic", use_periodic_, false);
00046     pnh_->param("use_message", use_message_, false);
00047     pnh_->param("use_trigger", use_trigger_, false);
00048     pnh_->param("periodic_rate", periodic_rate_, 10.0);
00049     
00050     bool frame_id_read_p
00051       = jsk_topic_tools::readVectorParameter(*pnh_, "frame_ids",
00052                                              frame_ids_);
00053     if (!frame_id_read_p) {
00054       JSK_NODELET_FATAL("failed to read frame_ids from ~frame_ids");
00055       return;
00056     }
00057     
00058     bool polygon_read_p = readPolygonArray("polygon_array");
00059     if (!polygon_read_p) {
00060       JSK_NODELET_FATAL("failed to read polygons from ~polygon_array");
00061       return;
00062     }
00063 
00064     if (frame_ids_.size() != polygons_.polygons.size()) {
00065       JSK_NODELET_FATAL("the size of frame_ids(%lu) does not match the size of polygons(%lu)",
00066                     frame_ids_.size(), polygons_.polygons.size());
00067       return;
00068     }
00069     else {
00070       for (size_t i = 0; i < frame_ids_.size(); i++) {
00071         polygons_.polygons[i].header.frame_id = frame_ids_[i];
00072         coefficients_.coefficients[i].header.frame_id = frame_ids_[i];
00073       }
00074     }
00075     
00076     if (!use_periodic_ && !use_message_ && !use_trigger_) {
00077       JSK_NODELET_FATAL("~use_preiodic, ~use_trigger nor ~use_message is not true");
00078       return;
00079     }
00080     polygons_.header.frame_id = frame_ids_[0];
00081     coefficients_.header.frame_id = frame_ids_[0];
00082 
00083     if (!use_periodic_) {
00084       polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(
00085         *pnh_, "output_polygons", 1);
00086       coefficients_pub_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00087         *pnh_, "output_coefficients", 1);
00088     }
00089     else {
00090       polygon_pub_ = pnh_->advertise<jsk_recognition_msgs::PolygonArray>(
00091         "output_polygons", 1);
00092       coefficients_pub_ = pnh_->advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00093         "output_coefficients", 1);
00094       subscribe();
00095       timer_ = pnh_->createTimer(ros::Duration(1.0 / periodic_rate_), &StaticPolygonArrayPublisher::timerCallback, this);
00096     }
00097   }
00098 
00099   void StaticPolygonArrayPublisher::subscribe()
00100   {
00101     if (use_message_) {
00102       sub_ = pnh_->subscribe("input", 1, &StaticPolygonArrayPublisher::inputCallback, this);
00103     }
00104     if (use_trigger_) {
00105       sub_input_.subscribe(*pnh_, "input", 1);
00106       sub_trigger_.subscribe(*pnh_, "trigger", 1);
00107       sync_
00108         = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00109       sync_->connectInput(sub_input_, sub_trigger_);
00110       sync_->registerCallback(boost::bind(
00111                                 &StaticPolygonArrayPublisher::triggerCallback,
00112                                 this, _1, _2));
00113     }
00114   }
00115 
00116   void StaticPolygonArrayPublisher::unsubscribe()
00117   {
00118     if (use_message_) {
00119       sub_.shutdown();
00120     }
00121     if (use_trigger_) {
00122       sub_input_.unsubscribe();
00123       sub_trigger_.unsubscribe();
00124     }
00125   }
00126 
00127   void StaticPolygonArrayPublisher::triggerCallback(
00128     const sensor_msgs::PointCloud2::ConstPtr& input,
00129     const jsk_recognition_msgs::Int32Stamped::ConstPtr& trigger)
00130   {
00131     publishPolygon(input->header.stamp);
00132   }
00133   
00134   PCLModelCoefficientMsg StaticPolygonArrayPublisher::polygonToModelCoefficients(const geometry_msgs::PolygonStamped& polygon)
00135   {
00136     Eigen::Vector3d A, B, C;
00137     A[0] = polygon.polygon.points[0].x;
00138     A[1] = polygon.polygon.points[0].y;
00139     A[2] = polygon.polygon.points[0].z;
00140     B[0] = polygon.polygon.points[1].x;
00141     B[1] = polygon.polygon.points[1].y;
00142     B[2] = polygon.polygon.points[1].z;
00143     C[0] = polygon.polygon.points[2].x;
00144     C[1] = polygon.polygon.points[2].y;
00145     C[2] = polygon.polygon.points[2].z;
00146     Eigen::Vector3d n = (B - A).cross(C - A).normalized();
00147     double a = n[0];
00148     double b = n[1];
00149     double c = n[2];
00150     double d = -(a * A[0] + b * A[1] + c * A[2]);
00151     PCLModelCoefficientMsg coefficient;
00152     coefficient.header = polygon.header;
00153     coefficient.values.push_back(a);
00154     coefficient.values.push_back(b);
00155     coefficient.values.push_back(c);
00156     coefficient.values.push_back(d);
00157     return coefficient;
00158   }
00159 
00160   /*
00161     parameter format is:
00162     polygon_array: [[[0, 0, 0], [0, 0, 1], [1, 0, 0]], ...]
00163    */
00164   bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name)
00165   {
00166     if (pnh_->hasParam(param_name)) {
00167       XmlRpc::XmlRpcValue v;
00168       pnh_->param(param_name, v, v);
00169       if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00170         for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) { // polygons
00171           XmlRpc::XmlRpcValue polygon_v = v[toplevel_i];
00172           geometry_msgs::PolygonStamped polygon;
00173           if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
00174               polygon_v.size() >= 3) {
00175             for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) { // each polygon, vertices
00176               XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i];
00177               if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
00178                   vertex_v.size() == 3 ) { // vertex_v := [x, y, z]
00179                 double x = getXMLDoubleValue(vertex_v[0]);
00180                 double y = getXMLDoubleValue(vertex_v[1]);
00181                 double z = getXMLDoubleValue(vertex_v[2]);
00182                 geometry_msgs::Point32 point;
00183                 point.x = x;
00184                 point.y = y;
00185                 point.z = z;
00186                 polygon.polygon.points.push_back(point);
00187               }
00188               else {
00189                 JSK_NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3",
00190                               param_name.c_str(), toplevel_i, secondlevel_i);
00191                 return false;
00192               }
00193             }
00194             polygons_.polygons.push_back(polygon);
00195             // estimate model coefficients
00196             coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon));
00197           }
00198           else {
00199             JSK_NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
00200             return false;
00201           }
00202         }
00203         return true;
00204       }
00205       else {
00206         JSK_NODELET_FATAL("%s is not array", param_name.c_str());
00207         return false;
00208       }
00209     }
00210     else {
00211       JSK_NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
00212       return false;
00213     }
00214     return true;
00215   }
00216   
00217   double StaticPolygonArrayPublisher::getXMLDoubleValue(XmlRpc::XmlRpcValue val) {
00218     switch(val.getType()) {
00219     case XmlRpc::XmlRpcValue::TypeInt:
00220       return (double)((int)val);
00221     case XmlRpc::XmlRpcValue::TypeDouble:
00222       return (double)val;
00223     default:
00224       return 0;
00225     }
00226   }
00227 
00228   
00229   void StaticPolygonArrayPublisher::inputCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00230   {
00231     publishPolygon(msg->header.stamp);
00232   }
00233   
00234   void StaticPolygonArrayPublisher::timerCallback(const ros::TimerEvent& event)
00235   {
00236     publishPolygon(event.current_expected);
00237   }
00238   
00239   void StaticPolygonArrayPublisher::publishPolygon(const ros::Time& stamp)
00240   {
00241     polygons_.header.stamp = stamp;
00242     for (size_t i = 0; i < polygons_.polygons.size(); i++) {
00243       polygons_.polygons[i].header.stamp = stamp;
00244     }
00245     
00246     coefficients_.header.stamp = stamp;
00247     for (size_t i = 0; i < coefficients_.coefficients.size(); i++) {
00248       coefficients_.coefficients[i].header.stamp = stamp;
00249     }
00250     
00251     polygon_pub_.publish(polygons_);
00252     coefficients_pub_.publish(coefficients_);
00253   }
00254 
00255 }
00256 
00257 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::StaticPolygonArrayPublisher,
00258                         nodelet::Nodelet);
00259 


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48