centroid_publisher_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ryohei Ueda and 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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include <jsk_pcl_ros_utils/centroid_publisher.h>
00037 
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <geometry_msgs/PoseArray.h>
00040 #include <geometry_msgs/PointStamped.h>
00041 
00042 #include <algorithm>
00043 #include <iomanip>
00044 #include <sstream>
00045 #include <jsk_recognition_utils/geo_util.h>
00046 #include <jsk_recognition_utils/pcl_conversion_util.h>
00047 
00048 namespace jsk_pcl_ros_utils
00049 {
00050   void CentroidPublisher::extract(const sensor_msgs::PointCloud2ConstPtr& input)
00051   {
00052     vital_checker_->poke();
00053     pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
00054     pcl::fromROSMsg(*input, cloud_xyz);
00055     Eigen::Vector4f center;
00056     bool is_valid = pcl::compute3DCentroid(cloud_xyz, center);
00057     if (publish_tf_ && is_valid) {
00058       tf::Transform transform;
00059       transform.setOrigin(tf::Vector3(center[0], center[1], center[2]));
00060       transform.setRotation(tf::createIdentityQuaternion());
00061       br_.sendTransform(tf::StampedTransform(transform, input->header.stamp,
00062                                              input->header.frame_id, frame_));
00063     }
00064     geometry_msgs::PoseStamped pose;
00065     pose.pose.orientation.w = 1.0;
00066     pose.pose.position.x = center[0];
00067     pose.pose.position.y = center[1];
00068     pose.pose.position.z = center[2];
00069     pose.header = input->header;
00070     pub_pose_.publish(pose);
00071     geometry_msgs::PointStamped point;
00072     point.point.x = center[0];
00073     point.point.y = center[1];
00074     point.point.z = center[2];
00075     point.header = input->header;
00076     pub_point_.publish(point);
00077   }
00078 
00079   void CentroidPublisher::extractPolygons(const jsk_recognition_msgs::PolygonArray::ConstPtr& input)
00080   {
00081     vital_checker_->poke();
00082 
00083     int size = std::min(100, (int)input->polygons.size());
00084 
00085     geometry_msgs::PoseArray pose_array;
00086     pose_array.header = input->header;
00087     pose_array.poses.resize(size);
00088 
00089     for (size_t i = 0; i < size; ++i) {
00090       jsk_recognition_utils::Polygon::Ptr polygon
00091         = jsk_recognition_utils::Polygon::fromROSMsgPtr(input->polygons[i].polygon);
00092       Eigen::Vector3f c = polygon->centroid(), n = polygon->getNormal();
00093       Eigen::Vector3f ez;
00094       ez << 0.0f, 0.0f, 1.0f;
00095       Eigen::Quaternionf q;
00096       q.setFromTwoVectors(ez, n);
00097 
00098       if (publish_tf_) {
00099         std::stringstream ss;
00100         ss << frame_ << std::setfill('0') << std::setw(2) << std::right << i;
00101         tf::Transform transform;
00102         transform.setOrigin(tf::Vector3(c.x(), c.y(), c.z()));
00103         transform.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w()));
00104         br_.sendTransform(tf::StampedTransform(transform, input->header.stamp,
00105                                                input->header.frame_id, ss.str()));
00106       }
00107 
00108       pose_array.poses[i].position.x = c.x();
00109       pose_array.poses[i].position.y = c.y();
00110       pose_array.poses[i].position.z = c.z();
00111       pose_array.poses[i].orientation.x = q.x();
00112       pose_array.poses[i].orientation.y = q.y();
00113       pose_array.poses[i].orientation.z = q.z();
00114       pose_array.poses[i].orientation.w = q.w();
00115     }
00116 
00117     pub_pose_array_.publish(pose_array);
00118   }
00119 
00120   void CentroidPublisher::subscribe()
00121   {
00122     sub_cloud_ = pnh_->subscribe("input", 1, &CentroidPublisher::extract, this);
00123     sub_polygons_ = pnh_->subscribe("input/polygons", 1, &CentroidPublisher::extractPolygons, this);
00124   }
00125 
00126   void CentroidPublisher::unsubscribe()
00127   {
00128     sub_cloud_.shutdown();
00129     sub_polygons_.shutdown();
00130   }
00131   
00132   void CentroidPublisher::onInit(void)
00133   {
00134     DiagnosticNodelet::onInit();
00135     pnh_->param("publish_tf", publish_tf_, false);
00136     if (publish_tf_) {
00137       if (!pnh_->getParam("frame", frame_))
00138       {
00139         ROS_WARN("~frame is not specified, using %s", getName().c_str());
00140         frame_ = getName();
00141       }
00142       // do not use DiagnosticNodelet functionality when ~publish_tf is false
00143       pub_pose_ = pnh_->advertise<geometry_msgs::PoseStamped>("output/pose", 1);
00144       pub_point_ = pnh_->advertise<geometry_msgs::PointStamped>(
00145         "output/point", 1);
00146       pub_pose_array_ = pnh_->advertise<geometry_msgs::PoseArray>("output/pose_array", 1);
00147       subscribe();
00148     }
00149     else {
00150       pub_pose_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose", 1);
00151       pub_point_ = advertise<geometry_msgs::PointStamped>(
00152         *pnh_, "output/point", 1);
00153       pub_pose_array_ = advertise<geometry_msgs::PoseArray>(*pnh_, "output/pose_array", 1);
00154     }
00155 
00156     onInitPostProcess();
00157   }
00158 }
00159 
00160 #include <pluginlib/class_list_macros.h>
00161 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::CentroidPublisher, nodelet::Nodelet);


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:40:52