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 #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
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
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);