pr2_marker_control: pr2_marker_control: cloud

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/kdtree/kdtree_flann.h>
00045 #include <pcl/filters/filter.h>
00046 
00047 #include <pr2_object_manipulation_msgs/CameraFocus.h>
00048 
00049 #include "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,
00058                             std::string name, InteractiveMarkerServer &server, 
00059                             object_manipulator::MechanismInterface &mechanism ) :
00060   name_(name)
00061   , nh_(nh)
00062   , tfl_(tfl)
00063   , marker_server_(server)
00064   , cloud_pts_   ( new pcl::PointCloud  <PointT> ()       )
00065   , cloud_normals_  ( new pcl::PointCloud  <pcl::Normal> ()  )
00066   , tree_(new pcl::KdTreeFLANN<PointT>())
00067   , mechanism_(mechanism)
00068   , cloud_server_client_("point_cloud_server_action", true)
00069 {
00070   ros::NodeHandle pnh("~");
00071   pnh.param<double>("voxel_size", voxel_size_, 0.002);
00072   pnh.param<std::string>("head_pointing_frame", head_pointing_frame_, "/default_head_pointing_frame");
00073   pnh.param<bool>("double_menu", double_menu_, false);
00074   
00075   pub_right_click_ = nh_->advertise<geometry_msgs::PoseStamped>("right_click_point", 1);
00076   pub_left_click_ = nh_->advertise<geometry_msgs::PoseStamped>("left_click_point", 1);
00077   pub_refresh_flag_ = nh_->advertise<std_msgs::String>("refresh_flag", 1);
00078   pub_focus_ = nh_->advertise<pr2_object_manipulation_msgs::CameraFocus>("camera_focus", 1);
00079   
00080   // This must come last!
00081   makeMenu();
00082 }
00083 
00084 CloudHandler::~CloudHandler()
00085 {
00086   marker_server_.erase(name_);
00087 }
00088 
00089 void CloudHandler::makeMenu()
00090 {
00091   // create menu
00092   menu_handler_.insert( "Broadcast click position",  boost::bind( &CloudHandler::menuPoint, this, _1) );
00093   if(double_menu_)
00094     menu_handler_.insert( "------------------------",  boost::bind( &CloudHandler::menuPoint, this, _1) );
00095   menu_handler_.insert( "Focus camera here",  boost::bind( &CloudHandler::menuFocus, this, _1) );
00096   if(double_menu_)
00097     menu_handler_.insert( "------------------------",  boost::bind( &CloudHandler::menuFocus, this, _1) );
00098   menu_handler_.insert( "Refresh",  boost::bind( &CloudHandler::refresh, this ) );
00099   if(double_menu_)
00100     menu_handler_.insert( "-------",  boost::bind( &CloudHandler::refresh, this ) );
00101   menu_handler_.insert( "Clear",    boost::bind( &CloudHandler::clear, this) );
00102   if(double_menu_)
00103     menu_handler_.insert( "-----",    boost::bind( &CloudHandler::clear, this) );
00104 }
00105 
00106 void CloudHandler::leftClickPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00107 {
00108   if(!feedback->mouse_point_valid)
00109   {
00110     ROS_WARN("Clicked point had an invalid position. Not looking there!");
00111     return;
00112   }
00113 
00114   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00115                          << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00116   
00117   geometry_msgs::PointStamped click_point;
00118   click_point.point = feedback->mouse_point;
00119   click_point.header = feedback->header;
00120   click_point.header.stamp = ros::Time(0);
00121   
00122   try
00123   {
00124     !mechanism_.pointHeadAction(click_point, head_pointing_frame_, false); 
00125   }
00126   catch (object_manipulator::ServiceNotFoundException &ex)
00127   {
00128     ROS_WARN("Can't point head, a needed service or action server was not found.");
00129   }
00130 }
00131 
00132 void CloudHandler::menuFocus( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00133 {
00134   if(!feedback->mouse_point_valid) return;
00135 
00136   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00137                          << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00138   
00139   geometry_msgs::PointStamped click_point;
00140   click_point.point = feedback->mouse_point;
00141   click_point.header = feedback->header;
00142   click_point.header.stamp = ros::Time(0);
00143   try{
00144     tfl_->transformPoint("/odom_combined", click_point, click_point );
00145   }
00146   catch(...)
00147   {
00148     ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00149               "/odom_combined", click_point.header.frame_id.c_str());
00150     return;
00151   }
00152   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00153                          << click_point.header.frame_id << " at point\n" << click_point.point );
00154   
00155   pr2_object_manipulation_msgs::CameraFocus msg;
00156   msg.focal_point = click_point;
00157   pub_focus_.publish(msg);
00158 }
00159 
00160 void CloudHandler::menuPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00161 {
00162   if(feedback->mouse_point_valid)
00163   {
00164     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00165                            << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00166 
00167     geometry_msgs::PointStamped click_point;
00168     click_point.point = feedback->mouse_point;
00169     click_point.header = feedback->header;
00170     click_point.header.stamp = ros::Time(0);
00171     geometry_msgs::PointStamped head_point;
00172     head_point.header.frame_id = "/head_tilt_link";
00173     try{
00174       tfl_->transformPoint("/odom_combined", click_point, click_point );
00175     }
00176     catch(...)
00177     {
00178       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00179                 "/odom_combined", click_point.header.frame_id.c_str());
00180       return;
00181     }
00182     try{
00183       tfl_->transformPoint("/odom_combined", head_point, head_point);
00184     }
00185     catch(...)
00186     {
00187       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00188                 "/odom_combined", head_point.header.frame_id.c_str());
00189       return;
00190     }
00191     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00192                            << click_point.header.frame_id << " at point\n" << click_point.point );
00193     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Head point in frame "
00194                            << head_point.header.frame_id << " at point\n" << head_point.point );
00195 
00196 
00197     PointT position; //(point.point.x, point.point.y, point.point.z);
00198     position.x = click_point.point.x;
00199     position.y = click_point.point.y;
00200     position.z = click_point.point.z;
00201     std::vector< int >          k_indices(1, 0);
00202     std::vector< float >  k_sqr_distances(1, 0);
00203     int N = tree_->nearestKSearch ( position, 1, k_indices, k_sqr_distances);
00204     if(!N)
00205     {
00206       ROS_ERROR("Found no point near clicked point... Serious error... Not broadcasting.");
00207       return;
00208     }
00209     int index = k_indices[0];
00210 
00211     geometry_msgs::PoseStamped ps;
00212     
00213     //PointT pt = cloud_pts_->points[index];
00214     //pcl::Normal norm = cloud_normals_->points[index];
00215 
00216     tf::Vector3 normal = tf::Vector3(cloud_normals_->points[index].normal_x,
00217                                      cloud_normals_->points[index].normal_y,
00218                                      cloud_normals_->points[index].normal_z);
00219 
00220     tf::Vector3 point = tf::Vector3(cloud_pts_->points[index].x,
00221                                     cloud_pts_->points[index].y,
00222                                     cloud_pts_->points[index].z);
00223 
00224     tf::Vector3 head = tf::Vector3(head_point.point.x, head_point.point.y, head_point.point.z);
00225     tf::Vector3 point_to_head = (head - point).normalized();
00226     // We actually want the normal that points away from the head
00227     if( point_to_head.dot(normal) < 0) normal *= -1;
00228 
00229     ps.pose.position.x = point.x();
00230     ps.pose.position.y = point.y();
00231     ps.pose.position.z = point.z();
00232     ROS_DEBUG_STREAM_NAMED("cloud_handler", "Nearest point at:\n" << ps.pose.position );
00233 
00234     tf::Vector3 Z = normal.normalized();
00235     tf::Vector3 Y = normal.cross(tf::Vector3(0,0,1)).normalized();
00236     tf::Vector3 X = Y.cross(normal).normalized();
00237     btMatrix3x3 mat(X.x(), Y.x(), Z.x(),
00238                     X.y(), Y.y(), Z.y(),
00239                     X.z(), Y.z(), Z.z());
00240     tf::Quaternion q;
00241     mat.getRotation(q);
00242     tf::quaternionTFToMsg(q, ps.pose.orientation);
00243     ps.header = click_point.header;
00244     pub_right_click_.publish(ps);
00245   }
00246   else
00247   {
00248     ROS_WARN("Clicked point had an invalid position. Not broadcasting.");
00249   }
00250 }
00251 
00252 void CloudHandler::saveCloudAndNormals()
00253 {
00254 
00255   //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
00256   pcl::fromROSMsg(msg_cloud_, *cloud_pts_);
00257   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());
00258 
00259   //std::vector<int> dummy_variable;
00260   //pcl::removeNaNFromPointCloud(*cloud_pts_, *cloud_pts_, dummy_variable);
00261   //ROS_DEBUG_NAMED("cloud_handler", "After removing NaNs cloud has %d points.", (int)cloud_pts_->points.size());
00262   //cloud_pts_->header = msg_cloud_.header;
00263 
00264 
00265   // Create a KD-Tree
00266   ROS_DEBUG_NAMED("cloud_handler", "Building kdtree...");
00267   ros::WallTime begin = ros::WallTime::now();
00268   //tree_.reset(new pcl::KdTreeFLANN<PointT> ());
00269   tree_->setInputCloud (cloud_pts_);
00270   //tree->addPointsFromInputCloud();
00271   ROS_DEBUG_NAMED("cloud_handler", "Initializing kdtree took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00272 
00273 
00274   cloud_normals_.reset(new pcl::PointCloud<pcl::Normal> ());
00275 
00276   begin = ros::WallTime::now();
00277   ROS_DEBUG_NAMED("cloud_handler", "Estimating normals...");
00278   pcl::NormalEstimationOMP<PointT, pcl::Normal> mls;
00279 
00280   // Set parameters
00281   mls.setInputCloud (cloud_pts_);
00282   mls.setSearchMethod (tree_);
00283   mls.setKSearch(20);
00284   // Reconstruct
00285   //ROS_DEBUG_NAMED("haptics", "computing...");
00286   mls.compute(*cloud_normals_);
00287   //ROS_DEBUG_NAMED("haptics", "...done!");
00288   ROS_DEBUG_NAMED("cloud_handler", "OMP Normal estimation took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00289 
00290   //  Need to remove NANs
00291 //    pcl::PassThrough<pcl::PointXYZRGB > sor;
00292 //    sor.setInputCloud (cloud);
00293 //    sor.setFilterLimits(-100, 100);
00294 //    sor.setFilterFieldName("z");
00295 //    sor.filter (*cloud);
00296 //    ROS_DEBUG_NAMED("cloud_handler", "After passthrough cloud has %d points.", (int)cloud->points.size());
00297 
00298 
00299 
00300 }
00301 
00302 void CloudHandler::get( sensor_msgs::PointCloud2 &cloud)
00303 {
00304   cloud = msg_cloud_;
00305 }
00306 
00307 sensor_msgs::PointCloud2 CloudHandler::get()
00308 {
00309   return msg_cloud_;
00310 }
00311 
00312 void CloudHandler::refresh()
00313 {
00314   ROS_DEBUG_NAMED("cloud_handler", "Processing menu-callback refresh, topic [%s]", topic_.c_str());
00315   refresh(topic_);
00316 }
00317 
00318 void CloudHandler::refresh(const std::string &topic)
00319 {
00320   topic_ = topic;
00321 
00322   ROS_DEBUG_NAMED("cloud_handler", "Sending request for cloud on topic [%s]", topic_.c_str());
00323   point_cloud_server::StoreCloudGoal cloud_goal;
00324   cloud_goal.action = cloud_goal.GET;
00325   cloud_goal.topic = topic_;
00326   cloud_goal.storage_frame_id =   "/odom_combined";
00327   cloud_goal.result_frame_id =    "/odom_combined";
00328   cloud_goal.name = "interactive_manipulation_snapshot";
00329   cloud_server_client_.sendGoalAndWait(cloud_goal, ros::Duration(15.0), ros::Duration(5.0));
00330 
00331   if(cloud_server_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00332   {
00333     ROS_DEBUG_NAMED("cloud_handler", "Got response from server!");
00334     ROS_DEBUG_NAMED("cloud_handler", "Downsampling cloud...");
00335     ros::WallTime begin = ros::WallTime::now();
00336     // Create the filtering object
00337     sensor_msgs::PointCloud2::Ptr ptr (new sensor_msgs::PointCloud2 (cloud_server_client_.getResult()->cloud ));
00338 
00339     //sensor_msgs::PointCloud2 temp =  cloud_server_client_.getResult()->cloud;
00340     //sensor_msgs::PointCloud2::Ptr ptr(&temp);
00341     pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
00342     sor.setInputCloud (ptr);
00343     sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00344     sor.filter (msg_cloud_);
00345     ROS_DEBUG_NAMED("cloud_handler", "Downsampling took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
00346     //msg_cloud_ = cloud_server_client_.getResult()->cloud;
00347     ptr.reset();
00348     saveCloudAndNormals();
00349     makeMarker(voxel_size_ * 1.8);
00350 
00351     std_msgs::String flag;
00352     flag.data = cloud_goal.name;
00353     pub_refresh_flag_.publish(flag);
00354   }
00355   else
00356   {
00357     ROS_DEBUG_NAMED("cloud_handler", "Server did not succeed, status %s", cloud_server_client_.getState().toString().c_str());
00358   }
00359 }
00360 
00361 void CloudHandler::clear()
00362 {
00363   marker_server_.erase(name_);
00364 }
00365 
00366 
00367 void CloudHandler::makeMarker(float size)
00368 {
00369   InteractiveMarker int_marker;
00370   int_marker.name = name_;
00371 
00372   Marker marker;
00373   marker.color.r = 1.0;
00374   marker.color.g = 1.0;
00375   marker.color.b = 1.0;
00376   marker.color.a = 1.0;
00377   marker.frame_locked = false;
00378 
00379   if(cloud_pts_->points.size())
00380   {
00381     int_marker.header = msg_cloud_.header;
00382     marker.scale.x = size;
00383     marker.scale.y = size;
00384     marker.scale.z = size;
00385     marker.type = visualization_msgs::Marker::SPHERE_LIST;
00386 
00387     int num_points = cloud_pts_->points.size();
00388     marker.points.resize( num_points );
00389     marker.colors.resize( num_points );
00390 
00391     //ROS_INFO_STREAM( "Adding point cluster. #points=" << object_.cluster.points.size() );
00392 
00393     //pcl::PointCloud<PointT>::Ptr &cloud = cloud_pts_;
00394     for ( int i=0; i<num_points; i++)
00395     {
00396       marker.points[i].x = cloud_pts_->points[i].x;
00397       marker.points[i].y = cloud_pts_->points[i].y;
00398       marker.points[i].z = cloud_pts_->points[i].z;
00399       marker.colors[i].r = cloud_pts_->points[i].r/255.;
00400       marker.colors[i].g = cloud_pts_->points[i].g/255.;
00401       marker.colors[i].b = cloud_pts_->points[i].b/255.;
00402       marker.colors[i].a = 1.0;
00403     }
00404   }
00405 
00406   InteractiveMarkerControl control;
00407   control.always_visible = true;
00408   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00409   control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00410 
00411   control.markers.push_back( marker );
00412 
00413   int_marker.controls.push_back( control );
00414 
00415   marker_server_.insert( int_marker, boost::bind( &CloudHandler::leftClickPoint, this, _1 ),
00416                          visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK);
00417   menu_handler_.apply( marker_server_, name_ );
00418   marker_server_.applyChanges();
00419 }
00420 
00421 
00422 //}
cloud_handler.cpp
