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_utils/normal_flip_to_frame.h"
00039 #include "jsk_recognition_utils/pcl_conversion_util.h"
00040
00041
00042 namespace jsk_pcl_ros_utils
00043 {
00044 void NormalFlipToFrame::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00048 tf_listener_ = jsk_recognition_utils::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 onInitPostProcess();
00055 }
00056
00057 void NormalFlipToFrame::subscribe()
00058 {
00059 sub_ = pnh_->subscribe("input", 1, &NormalFlipToFrame::flip, this);
00060 }
00061
00062 void NormalFlipToFrame::unsubscribe()
00063 {
00064 sub_.shutdown();
00065 }
00066
00067 void NormalFlipToFrame::flip(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00068 {
00069 vital_checker_->poke();
00070 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud
00071 (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00072 pcl::fromROSMsg(*cloud_msg, *cloud);
00073
00074 try {
00075 ros::Time stamp;
00076 if (strict_tf_) {
00077 stamp = cloud_msg->header.stamp;
00078 }
00079 else {
00080 stamp = ros::Time(0);
00081 }
00082 tf::StampedTransform sensor_transform_tf
00083 = jsk_recognition_utils::lookupTransformWithDuration(
00084 tf_listener_, frame_id_,
00085 cloud_msg->header.frame_id,
00086 stamp,
00087 ros::Duration(1.0));
00088 Eigen::Affine3f sensor_transform;
00089 tf::transformTFToEigen(sensor_transform_tf, sensor_transform);
00090 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr flipped_cloud
00091 (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00092 flipped_cloud->points.resize(cloud->points.size());
00093 Eigen::Vector3f s(sensor_transform.translation());
00094 for (size_t i = 0; i < cloud->points.size(); i++) {
00095
00096 if (pcl_isfinite(cloud->points[i].x) &&
00097 pcl_isfinite(cloud->points[i].y) &&
00098 pcl_isfinite(cloud->points[i].z) &&
00099 pcl_isfinite(cloud->points[i].normal_x) &&
00100 pcl_isfinite(cloud->points[i].normal_y) &&
00101 pcl_isfinite(cloud->points[i].normal_z)) {
00102 Eigen::Vector3f p = cloud->points[i].getVector3fMap();
00103 Eigen::Vector3f n(cloud->points[i].normal_x,
00104 cloud->points[i].normal_y,
00105 cloud->points[i].normal_z);
00106 if ((s - p).dot(n) < 0) {
00107 pcl::PointXYZRGBNormal new_p;
00108 jsk_recognition_utils::pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZRGBNormal>(
00109 p, new_p);
00110 new_p.rgb = cloud->points[i].rgb;
00111 new_p.normal_x = - n[0];
00112 new_p.normal_y = - n[1];
00113 new_p.normal_z = - n[2];
00114 flipped_cloud->points[i] = new_p;
00115 }
00116 else {
00117 flipped_cloud->points[i] = cloud->points[i];
00118 }
00119 }
00120 else {
00121 flipped_cloud->points[i] = cloud->points[i];
00122 }
00123
00124 }
00125 sensor_msgs::PointCloud2 ros_cloud;
00126 pcl::toROSMsg(*flipped_cloud, ros_cloud);
00127 ros_cloud.header = cloud_msg->header;
00128 pub_.publish(ros_cloud);
00129 }
00130 catch (tf2::ConnectivityException &e)
00131 {
00132 NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00133 }
00134 catch (tf2::InvalidArgumentException &e)
00135 {
00136 NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00137 }
00138 catch (tf2::TransformException &e)
00139 {
00140 NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00141 }
00142 }
00143 }
00144
00145 #include <pluginlib/class_list_macros.h>
00146 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::NormalFlipToFrame, nodelet::Nodelet);