pcl_normal_visualization.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/PointCloud2.h>
00003 
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/features/normal_3d.h>
00007 #include <pcl/features/boundary.h>
00008 #include <pcl/segmentation/extract_clusters.h>
00009 #include <pcl/filters/extract_indices.h>
00010 #include <pcl/filters/passthrough.h>
00011 
00012 #include "LinearMath/btTransform.h"
00013 
00014 #include <pcl_ros/publisher.h>
00015 
00016 #include <tf/transform_listener.h>
00017 
00018 #include <visualization_msgs/MarkerArray.h>
00019 
00020 #include <vector>
00021 
00037 class NormalViz
00038 {
00039 protected:
00040   ros::NodeHandle nh_;
00041 
00042   //parameters
00043   std::string input_cloud_topic_;
00044 
00045   sensor_msgs::PointCloud2 cloud_in_;
00046   visualization_msgs::MarkerArray normals_marker_array_msg_;
00047 
00048   ros::Subscriber cloud_sub_;
00049   ros::Publisher normals_marker_array_publisher_;
00050   ros::Publisher normals_marker_publisher_;
00051 
00052   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg);
00053 
00054 public:
00055   NormalViz(ros::NodeHandle &anode);
00056   ~NormalViz();
00057 };
00058 
00059 
00060 NormalViz::NormalViz (ros::NodeHandle &anode) : nh_(anode) {
00061   nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/points2_out"));
00062 
00063   cloud_sub_ = nh_.subscribe (input_cloud_topic_, 1, &NormalViz::cloud_cb, this);
00064 
00065   normals_marker_array_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>("normals_marker_array", 100);
00066   normals_marker_publisher_ = nh_.advertise<visualization_msgs::Marker>("normals_marker", 100);
00067 }
00068 
00069 
00070 NormalViz::~NormalViz()
00071 {
00072   ROS_INFO("Shutting down pcl_normal_visualization node!");
00073 
00074   normals_marker_array_publisher_.shutdown();
00075   normals_marker_publisher_.shutdown();
00076 }
00077 
00078 
00083 void NormalViz::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc2_msg) {
00084 
00085   static unsigned int lastsize = 0;
00086 
00087   ROS_INFO("Received PointCloud message.");
00088 
00089   unsigned int validfields = 0;
00090   for (unsigned int i = 0; i < pc2_msg->fields.size(); i++) {
00091     if (!strcmp(pc2_msg->fields[i].name.c_str(), "x"))
00092       validfields++;
00093     else if (!strcmp(pc2_msg->fields[i].name.c_str(), "y"))
00094       validfields++;
00095     else if (!strcmp(pc2_msg->fields[i].name.c_str(), "z"))
00096       validfields++;
00097     else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_x"))
00098       validfields++;
00099     else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_y"))
00100       validfields++;
00101     else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_z"))
00102       validfields++;
00103     ROS_INFO("read field: %s", pc2_msg->fields[i].name.c_str());
00104   }
00105 
00106   //don't process if neccessary field were not found
00107   if ( validfields != 6 ) {
00108     ROS_INFO("PointCloud message does not contain neccessary fields!");
00109     return;
00110   }
00111 
00112   //Converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
00113   pcl::PointCloud<pcl::PointNormal> pcl_cloud;
00114   pcl::fromROSMsg(*pc2_msg, pcl_cloud);
00115 
00116   unsigned int size = pc2_msg->height * pc2_msg->width;
00117   ROS_INFO("size: %i", size);
00118   if (size >= lastsize) {
00119     normals_marker_array_msg_.markers.resize(size);
00120   }
00121 
00122   for (unsigned int i = 0; i < size; ++i)
00123     {
00124       geometry_msgs::Point pos;
00125       pos.x = pcl_cloud.points[i].x;
00126       pos.y = pcl_cloud.points[i].y;
00127       pos.z = pcl_cloud.points[i].z;
00128       normals_marker_array_msg_.markers[i].pose.position = pos;
00129       //axis-angle rotation
00130       btVector3 axis(pcl_cloud.points[i].normal[0],pcl_cloud.points[i].normal[1],pcl_cloud.points[i].normal[2]);
00131       btVector3 marker_axis(1, 0, 0);
00132       btQuaternion qt(marker_axis.cross(axis.normalize()), marker_axis.angle(axis.normalize()));
00133       geometry_msgs::Quaternion quat_msg;
00134       tf::quaternionTFToMsg(qt, quat_msg);
00135       normals_marker_array_msg_.markers[i].pose.orientation = quat_msg;
00136 
00137       normals_marker_array_msg_.markers[i].header.frame_id = pcl_cloud.header.frame_id;
00138       normals_marker_array_msg_.markers[i].header.stamp = pcl_cloud.header.stamp;
00139       normals_marker_array_msg_.markers[i].id = i;
00140       normals_marker_array_msg_.markers[i].ns = "Normals";
00141       normals_marker_array_msg_.markers[i].color.r = 1.0f;
00142       normals_marker_array_msg_.markers[i].color.g = 0.0f;
00143       normals_marker_array_msg_.markers[i].color.b = 0.0f;
00144       normals_marker_array_msg_.markers[i].color.a = 0.5f;
00145       normals_marker_array_msg_.markers[i].lifetime = ros::Duration::Duration();
00146       normals_marker_array_msg_.markers[i].type = visualization_msgs::Marker::ARROW;
00147       normals_marker_array_msg_.markers[i].scale.x = 0.2;
00148       normals_marker_array_msg_.markers[i].scale.y = 0.2;
00149       normals_marker_array_msg_.markers[i].scale.z = 0.2;
00150 
00151       normals_marker_array_msg_.markers[i].action = visualization_msgs::Marker::ADD;
00152     }
00153 
00154   if (lastsize > size) {
00155     for (unsigned int i = size; i < lastsize; ++i) {
00156       normals_marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
00157     }
00158   }
00159   lastsize = size;
00160 
00161   normals_marker_array_publisher_.publish(normals_marker_array_msg_);
00162 
00163   ROS_INFO("Published marker array with normals");
00164 }
00165 
00166 int main (int argc, char* argv[])
00167 {
00168   ros::init (argc, argv, "pcl_normal_visualization");
00169   ros::NodeHandle nh("~");
00170   NormalViz n (nh);
00171   ROS_INFO("Node up and running...");
00172   ros::spin ();
00173 
00174   return (0);
00175 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Thu May 23 2013 17:11:36