cloud_handler.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // author: Adam Leeper
00031 
00032 #include <visualization_msgs/InteractiveMarker.h>
00033 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00034 #include <geometry_msgs/PoseStamped.h>
00035 #include <std_msgs/String.h>
00036 #include <tf/tf.h>
00037 #include <point_cloud_server/StoreCloudAction.h>
00038 
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/point_types.h>
00041 //#include <pcl/filters/passthrough.h>
00042 #include <pcl/filters/voxel_grid.h>
00043 #include <pcl/features/normal_3d_omp.h>
00044 #include <pcl/search/kdtree.h>
00045 #include <pcl/filters/filter.h>
00046 
00047 #include <pr2_object_manipulation_msgs/CameraFocus.h>
00048 
00049 #include <pr2_marker_control/cloud_handler.h>
00050 
00051 using namespace visualization_msgs;
00052 using namespace interactive_markers;
00053 
00054 
00055 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00056 
00057 CloudHandler::CloudHandler( ros::NodeHandle *nh, tf::TransformListener *tfl, std::string marker_name, 
00058                             std::string topic_name, std::string server_name,
00059                             object_manipulator::MechanismInterface &mechanism,
00060                             std::string cloud_frame):
00061   marker_name_(marker_name)
00062   , nh_(nh)
00063   , tfl_(tfl)
00064   , marker_server_(topic_name, server_name, false)
00065   , cloud_pts_   ( new pcl::PointCloud  <PointT> ()       )
00066   , cloud_normals_  ( new pcl::PointCloud  <pcl::Normal> ()  )
00067   , tree_(new pcl::search::KdTree<PointT>())
00068   , mechanism_(mechanism)
00069   , cloud_server_client_("point_cloud_server_action", true)
00070   , cloud_frame_(cloud_frame)
00071 {
00072   ros::NodeHandle pnh("~");
00073   pnh.param<double>("voxel_size", voxel_size_, 0.002);
00074   pnh.param<std::string>("head_pointing_frame", head_pointing_frame_, "/default_head_pointing_frame");
00075   
00076   pub_right_click_ = nh_->advertise<geometry_msgs::PoseStamped>("right_click_point", 1);
00077   pub_left_click_ = nh_->advertise<geometry_msgs::PoseStamped>("left_click_point", 1);
00078   pub_refresh_flag_ = nh_->advertise<std_msgs::String>("refresh_flag", 1);
00079   pub_focus_ = nh_->advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1);
00080   
00081   // This must come last!
00082   makeMenu();
00083 }
00084 
00085 CloudHandler::~CloudHandler()
00086 {
00087   marker_server_.erase(marker_name_);
00088 }
00089 
00090 void CloudHandler::makeMenu()
00091 {
00092   // create menu
00093   menu_handler_.insert( "Broadcast click position",  boost::bind( &CloudHandler::menuPoint, this, _1) );
00094   menu_handler_.insert( "Focus camera here",  boost::bind( &CloudHandler::menuFocus, this, _1) );
00095   menu_handler_.insert( "Refresh",  boost::bind( &CloudHandler::refresh, this ) );
00096   menu_handler_.insert( "Clear",    boost::bind( &CloudHandler::clear, this) );
00097 }
00098 
00099 void CloudHandler::leftClickPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00100 {
00101   if(!feedback->mouse_point_valid)
00102   {
00103     ROS_WARN("Clicked point had an invalid position. Not looking there!");
00104     return;
00105   }
00106 
00107   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00108                          << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00109   
00110   geometry_msgs::PointStamped click_point;
00111   click_point.point = feedback->mouse_point;
00112   click_point.header = feedback->header;
00113   click_point.header.stamp = ros::Time(0);
00114   
00115   try
00116   {
00117     !mechanism_.pointHeadAction(click_point, head_pointing_frame_, false); 
00118   }
00119   catch (object_manipulator::ServiceNotFoundException &ex)
00120   {
00121     ROS_WARN("Can't point head, a needed service or action server was not found.");
00122   }
00123 }
00124 
00125 void CloudHandler::menuFocus( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00126 {
00127   if(!feedback->mouse_point_valid) return;
00128 
00129   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00130                          << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00131   
00132   geometry_msgs::PointStamped click_point;
00133   click_point.point = feedback->mouse_point;
00134   click_point.header = feedback->header;
00135   click_point.header.stamp = ros::Time(0);
00136   try{
00137     tfl_->transformPoint(cloud_frame_, click_point, click_point );
00138   }
00139   catch(...)
00140   {
00141     ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00142               cloud_frame_.c_str(), click_point.header.frame_id.c_str());
00143     return;
00144   }
00145   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00146                          << click_point.header.frame_id << " at point\n" << click_point.point );
00147   
00148   pr2_object_manipulation_msgs::CameraFocus msg;
00149   msg.focal_point = click_point;
00150   pub_focus_.publish(msg);
00151 }
00152 
00153 void CloudHandler::menuPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00154 {
00155   if(feedback->mouse_point_valid)
00156   {
00157     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00158                            << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00159 
00160     geometry_msgs::PointStamped click_point;
00161     click_point.point = feedback->mouse_point;
00162     click_point.header = feedback->header;
00163     click_point.header.stamp = ros::Time(0);
00164     geometry_msgs::PointStamped head_point;
00165     head_point.header.frame_id = "/head_tilt_link";
00166     try{
00167       tfl_->transformPoint(cloud_frame_, click_point, click_point );
00168     }
00169     catch(...)
00170     {
00171       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00172                 cloud_frame_.c_str(), click_point.header.frame_id.c_str());
00173       return;
00174     }
00175     try{
00176       tfl_->transformPoint(cloud_frame_, head_point, head_point);
00177     }
00178     catch(...)
00179     {
00180       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00181                 cloud_frame_.c_str(), head_point.header.frame_id.c_str());
00182       return;
00183     }
00184     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00185                            << click_point.header.frame_id << " at point\n" << click_point.point );
00186     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Head point in frame "
00187                            << head_point.header.frame_id << " at point\n" << head_point.point );
00188 
00189     PointT position; //(point.point.x, point.point.y, point.point.z);
00190     position.x = click_point.point.x;
00191     position.y = click_point.point.y;
00192     position.z = click_point.point.z;
00193     std::vector< int >          k_indices(1, 0);
00194     std::vector< float >  k_sqr_distances(1, 0);
00195     int N = tree_->nearestKSearch ( position, 1, k_indices, k_sqr_distances);
00196     if(!N)
00197     {
00198       ROS_ERROR("Found no point near clicked point... Serious error... Not broadcasting.");
00199       return;
00200     }
00201     int index = k_indices[0];
00202 
00203     geometry_msgs::PoseStamped ps;
00204     
00205     //PointT pt = cloud_pts_->points[index];
00206     //pcl::Normal norm = cloud_normals_->points[index];
00207 
00208     tf::Vector3 normal = tf::Vector3(cloud_normals_->points[index].normal_x,
00209                                      cloud_normals_->points[index].normal_y,
00210                                      cloud_normals_->points[index].normal_z);
00211 
00212     tf::Vector3 point = tf::Vector3(cloud_pts_->points[index].x,
00213                                     cloud_pts_->points[index].y,
00214                                     cloud_pts_->points[index].z);
00215 
00216     tf::Vector3 head = tf::Vector3(head_point.point.x, head_point.point.y, head_point.point.z);
00217     tf::Vector3 point_to_head = (head - point).normalized();
00218     // We actually want the normal that points away from the head
00219     if( point_to_head.dot(normal) < 0) normal *= -1;
00220 
00221     ps.pose.position.x = point.x();
00222     ps.pose.position.y = point.y();
00223     ps.pose.position.z = point.z();
00224     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Nearest point at:\n" << ps.pose.position );
00225 
00226     tf::Vector3 Z = normal.normalized();
00227     tf::Vector3 Y = normal.cross(tf::Vector3(0,0,1)).normalized();
00228     tf::Vector3 X = Y.cross(normal).normalized();
00229     tf::Matrix3x3 mat(X.x(), Y.x(), Z.x(),
00230                     X.y(), Y.y(), Z.y(),
00231                     X.z(), Y.z(), Z.z());
00232     tf::Quaternion q;
00233     mat.getRotation(q);
00234     tf::quaternionTFToMsg(q, ps.pose.orientation);
00235     ps.header = click_point.header;
00236     pub_right_click_.publish(ps);
00237   }
00238   else
00239   {
00240     ROS_WARN("Clicked point had an invalid position. Not broadcasting.");
00241   }
00242 }
00243 
00244 void CloudHandler::saveCloudAndNormals()
00245 {
00246 
00247   //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
00248   pcl::fromROSMsg(msg_cloud_, *cloud_pts_);
00249   ROS_DEBUG_NAMED("cloud_handler", "Input cloud in frame [%s] has %d points.", cloud_pts_->header.frame_id.c_str(), (int)cloud_pts_->points.size());
00250 
00251   //std::vector<int> dummy_variable;
00252   //pcl::removeNaNFromPointCloud(*cloud_pts_, *cloud_pts_, dummy_variable);
00253   //ROS_DEBUG_NAMED("cloud_handler", "After removing NaNs cloud has %d points.", (int)cloud_pts_->points.size());
00254   //cloud_pts_->header = msg_cloud_.header;
00255 
00256 
00257   // Create a KD-Tree
00258   ROS_DEBUG_NAMED("cloud_handler", "Building kdtree...");
00259   ros::WallTime begin = ros::WallTime::now();
00260   tree_.reset(new pcl::search::KdTree<PointT> ());
00261   tree_->setInputCloud (cloud_pts_);
00262   //tree->addPointsFromInputCloud();
00263   ROS_DEBUG_NAMED("cloud_handler", "Initializing kdtree took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00264 
00265 
00266   cloud_normals_.reset(new pcl::PointCloud<pcl::Normal> ());
00267 
00268   begin = ros::WallTime::now();
00269   ROS_DEBUG_NAMED("cloud_handler", "Estimating normals...");
00270   pcl::NormalEstimationOMP<PointT, pcl::Normal> mls;
00271 
00272   // Set parameters
00273   mls.setInputCloud (cloud_pts_);
00274   mls.setSearchMethod (tree_);
00275   mls.setKSearch(20);
00276   // Reconstruct
00277   //ROS_DEBUG_NAMED("haptics", "computing...");
00278   mls.compute(*cloud_normals_);
00279   //ROS_DEBUG_NAMED("haptics", "...done!");
00280   ROS_DEBUG_NAMED("cloud_handler", "OMP Normal estimation took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00281 
00282   //  Need to remove NANs
00283 //    pcl::PassThrough<pcl::PointXYZRGB > sor;
00284 //    sor.setInputCloud (cloud);
00285 //    sor.setFilterLimits(-100, 100);
00286 //    sor.setFilterFieldName("z");
00287 //    sor.filter (*cloud);
00288 //    ROS_DEBUG_NAMED("cloud_handler", "After passthrough cloud has %d points.", (int)cloud->points.size());
00289 
00290 
00291 
00292 }
00293 
00294 void CloudHandler::get( sensor_msgs::PointCloud2 &cloud)
00295 {
00296   cloud = msg_cloud_;
00297 }
00298 
00299 sensor_msgs::PointCloud2 CloudHandler::get()
00300 {
00301   return msg_cloud_;
00302 }
00303 
00304 void CloudHandler::updateCloud(sensor_msgs::PointCloud2 cloud, std::string name)
00305 {
00306     ROS_DEBUG_NAMED("cloud_handler", "Downsampling cloud...");
00307     ros::WallTime begin = ros::WallTime::now();
00308     // Create the filtering object
00309     sensor_msgs::PointCloud2::Ptr ptr (new sensor_msgs::PointCloud2 (cloud));
00310 
00311     //sensor_msgs::PointCloud2 temp =  cloud_server_client_.getResult()->cloud;
00312     //sensor_msgs::PointCloud2::Ptr ptr(&temp);
00313     pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
00314     sor.setInputCloud (ptr);
00315     sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00316     sor.filter (msg_cloud_);
00317     ROS_DEBUG_NAMED("cloud_handler", "Downsampling took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00318     //msg_cloud_ = cloud_server_client_.getResult()->cloud;
00319     ptr.reset();
00320     saveCloudAndNormals();
00321     makeMarker(voxel_size_ * 1.8);
00322 
00323     std_msgs::String flag;
00324     flag.data = name;
00325     pub_refresh_flag_.publish(flag);
00326 }
00327 
00328 void CloudHandler::refresh()
00329 {
00330   ROS_DEBUG_NAMED("cloud_handler", "Processing menu-callback refresh, topic [%s]", topic_.c_str());
00331   refresh(topic_);
00332 }
00333 
00334 void CloudHandler::refresh(const std::string &topic)
00335 {
00336   topic_ = topic;
00337 
00338   ROS_DEBUG_NAMED("cloud_handler", "Sending request for cloud on topic [%s]", topic_.c_str());
00339   point_cloud_server::StoreCloudGoal cloud_goal;
00340   cloud_goal.action = cloud_goal.GET;
00341   cloud_goal.topic = topic_;
00342   cloud_goal.storage_frame_id =   cloud_frame_;
00343   cloud_goal.result_frame_id =    cloud_frame_;
00344   cloud_goal.name = "interactive_manipulation_snapshot";
00345   cloud_server_client_.sendGoalAndWait(cloud_goal, ros::Duration(15.0), ros::Duration(5.0));
00346 
00347   if(cloud_server_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00348   {
00349     ROS_DEBUG_NAMED("cloud_handler", "Got response from server!");
00350     updateCloud(cloud_server_client_.getResult()->cloud, cloud_goal.name);
00351   }
00352   else
00353   {
00354     ROS_DEBUG_NAMED("cloud_handler", "Server did not succeed, status %s", cloud_server_client_.getState().toString().c_str());
00355   }
00356 }
00357 
00358 void CloudHandler::clear()
00359 {
00360   marker_server_.erase(marker_name_);
00361   marker_server_.applyChanges();
00362 }
00363 
00364 
00365 void CloudHandler::makeMarker(float size)
00366 {
00367   InteractiveMarker int_marker;
00368   int_marker.name = marker_name_;
00369 
00370   Marker marker;
00371   marker.color.r = 1.0;
00372   marker.color.g = 1.0;
00373   marker.color.b = 1.0;
00374   marker.color.a = 1.0;
00375   marker.frame_locked = false;
00376 
00377   if(cloud_pts_->points.size())
00378   {
00379     int_marker.header = msg_cloud_.header;
00380     int_marker.header.stamp = ros::Time(0);
00381     marker.scale.x = size;
00382     marker.scale.y = size;
00383     marker.scale.z = size;
00384     marker.type = visualization_msgs::Marker::SPHERE_LIST;
00385 
00386     int num_points = cloud_pts_->points.size();
00387     marker.points.resize( num_points );
00388     marker.colors.resize( num_points );
00389 
00390     //ROS_INFO_STREAM( "Adding point cluster. #points=" << num_points << " frame=" << msg_cloud_.header.frame_id);
00391 
00392     //pcl::PointCloud<PointT>::Ptr &cloud = cloud_pts_;
00393     for ( int i=0; i<num_points; i++)
00394     {
00395       marker.points[i].x = cloud_pts_->points[i].x;
00396       marker.points[i].y = cloud_pts_->points[i].y;
00397       marker.points[i].z = cloud_pts_->points[i].z;
00398       marker.colors[i].r = cloud_pts_->points[i].r/255.;
00399       marker.colors[i].g = cloud_pts_->points[i].g/255.;
00400       marker.colors[i].b = cloud_pts_->points[i].b/255.;
00401       marker.colors[i].a = 1.0;
00402     }
00403   }
00404 
00405   InteractiveMarkerControl control;
00406   control.always_visible = true;
00407   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00408   control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00409 
00410   control.markers.push_back( marker );
00411 
00412   int_marker.controls.push_back( control );
00413 
00414   marker_server_.insert( int_marker, boost::bind( &CloudHandler::leftClickPoint, this, _1 ),
00415                          visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK);
00416   menu_handler_.apply( marker_server_, marker_name_ );
00417   marker_server_.applyChanges();
00418   ROS_INFO("made interactive marker for cloud");
00419 }
00420 
00421 
00422 //}


pr2_marker_control
Author(s): Adam Leeper
autogenerated on Mon Oct 6 2014 12:48:07