normal_flip_to_frame_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include "jsk_pcl_ros/normal_flip_to_frame.h"
00039 #include "jsk_pcl_ros/pcl_conversion_util.h"
00040 
00041 
00042 namespace jsk_pcl_ros
00043 {
00044   void NormalFlipToFrame::onInit()
00045   {
00046     DiagnosticNodelet::onInit();
00047     pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00048     tf_listener_ = TfListenerSingleton::getInstance();
00049     if (!pnh_->getParam("frame_id", frame_id_)) {
00050       NODELET_FATAL("[%s] no ~frame_id is specified", __PRETTY_FUNCTION__);
00051     }
00052     pnh_->param("strict_tf", strict_tf_, false);
00053     pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00054   }
00055 
00056   void NormalFlipToFrame::subscribe()
00057   {
00058     sub_ = pnh_->subscribe("input", 1, &NormalFlipToFrame::flip, this);
00059   }
00060 
00061   void NormalFlipToFrame::unsubscribe()
00062   {
00063     sub_.shutdown();
00064   }
00065 
00066   void NormalFlipToFrame::flip(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00067   {
00068     vital_checker_->poke();
00069     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud
00070       (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00071     pcl::fromROSMsg(*cloud_msg, *cloud);
00072     // check tf is available
00073     try {
00074       ros::Time stamp;
00075       if (strict_tf_) {
00076         stamp = cloud_msg->header.stamp;
00077       }
00078       else {
00079         stamp = ros::Time(0);
00080       }
00081       tf::StampedTransform sensor_transform_tf
00082         = lookupTransformWithDuration(
00083           tf_listener_, frame_id_,
00084           cloud_msg->header.frame_id,
00085           stamp,
00086           ros::Duration(1.0));
00087       Eigen::Affine3f sensor_transform;
00088       tf::transformTFToEigen(sensor_transform_tf, sensor_transform);
00089       pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr flipped_cloud
00090         (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00091       flipped_cloud->points.resize(cloud->points.size());
00092       Eigen::Vector3f s(sensor_transform.translation());
00093       for (size_t i = 0; i < cloud->points.size(); i++) {
00094         // Check point is not nan
00095         if (pcl_isfinite(cloud->points[i].x) &&
00096             pcl_isfinite(cloud->points[i].y) &&
00097             pcl_isfinite(cloud->points[i].z) &&
00098             pcl_isfinite(cloud->points[i].normal_x) &&
00099             pcl_isfinite(cloud->points[i].normal_y) &&
00100             pcl_isfinite(cloud->points[i].normal_z)) {
00101           Eigen::Vector3f p = cloud->points[i].getVector3fMap();
00102           Eigen::Vector3f n(cloud->points[i].normal_x,
00103                             cloud->points[i].normal_y,
00104                             cloud->points[i].normal_z);
00105           if ((s - p).dot(n) < 0) {
00106             pcl::PointXYZRGBNormal new_p;
00107             pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZRGBNormal>(
00108               p, new_p);
00109             new_p.rgb = cloud->points[i].rgb;
00110             new_p.normal_x = - n[0];
00111             new_p.normal_y = - n[1];
00112             new_p.normal_z = - n[2];
00113             flipped_cloud->points[i] = new_p;
00114           }
00115           else {
00116             flipped_cloud->points[i] = cloud->points[i];
00117           }
00118         }
00119         else {
00120           flipped_cloud->points[i] = cloud->points[i];
00121         }
00122         
00123       }
00124       sensor_msgs::PointCloud2 ros_cloud;
00125       pcl::toROSMsg(*flipped_cloud, ros_cloud);
00126       ros_cloud.header = cloud_msg->header;
00127       pub_.publish(ros_cloud);
00128     }
00129     catch (tf2::ConnectivityException &e)
00130     {
00131       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00132     }
00133     catch (tf2::InvalidArgumentException &e)
00134     {
00135       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00136     }
00137     catch (tf2::TransformException &e)
00138     {
00139       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00140     }
00141   }
00142 }
00143 
00144 #include <pluginlib/class_list_macros.h>
00145 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::NormalFlipToFrame, nodelet::Nodelet);


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