36 #define BOOST_PARAMETER_MAX_ARITY 7 46 DiagnosticNodelet::onInit();
47 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
50 NODELET_FATAL(
"[%s] no ~frame_id is specified", __PRETTY_FUNCTION__);
53 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
70 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
cloud 71 (
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
77 stamp = cloud_msg->header.stamp;
85 cloud_msg->header.frame_id,
88 Eigen::Affine3f sensor_transform;
90 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr flipped_cloud
91 (
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
92 flipped_cloud->points.resize(cloud->points.size());
93 Eigen::Vector3f
s(sensor_transform.translation());
94 for (
size_t i = 0; i < cloud->points.size(); i++) {
96 if (pcl_isfinite(cloud->points[i].x) &&
97 pcl_isfinite(cloud->points[i].y) &&
98 pcl_isfinite(cloud->points[i].z) &&
99 pcl_isfinite(cloud->points[i].normal_x) &&
100 pcl_isfinite(cloud->points[i].normal_y) &&
101 pcl_isfinite(cloud->points[i].normal_z)) {
102 Eigen::Vector3f
p = cloud->points[i].getVector3fMap();
103 Eigen::Vector3f n(cloud->points[i].normal_x,
104 cloud->points[i].normal_y,
105 cloud->points[i].normal_z);
106 if ((
s - p).dot(n) < 0) {
107 pcl::PointXYZRGBNormal new_p;
108 jsk_recognition_utils::pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZRGBNormal>(
110 new_p.rgb = cloud->points[i].rgb;
111 new_p.normal_x = - n[0];
112 new_p.normal_y = - n[1];
113 new_p.normal_z = - n[2];
114 flipped_cloud->points[i] = new_p;
117 flipped_cloud->points[i] = cloud->points[i];
121 flipped_cloud->points[i] = cloud->points[i];
125 sensor_msgs::PointCloud2 ros_cloud;
127 ros_cloud.header = cloud_msg->header;
132 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
136 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
140 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
tf::TransformListener * tf_listener_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::NormalFlipToFrame, nodelet::Nodelet)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void flip(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void unsubscribe()
static tf::TransformListener * getInstance()
#define NODELET_FATAL(...)