object_segmentation_gui: object_segmentation_gui: tabletop_object_segmentation_gui

00001 /*
00002  * Copyright (c) 2009, 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 #include <ros/ros.h>
00031 
00032 #include <actionlib/client/simple_action_client.h>
00033 
00034 #include <sensor_msgs/Image.h>
00035 #include <sensor_msgs/CameraInfo.h>
00036 #include <stereo_msgs/DisparityImage.h>
00037 
00038 #include <visualization_msgs/Marker.h>
00039 
00040 #include <tabletop_object_detector/TabletopSegmentation.h>
00041 
00042 #include <tf/transform_listener.h>
00043 
00044 #include "object_segmentation_gui/ObjectSegmentationGuiAction.h"
00045 
00046 
00047 class TabletopObjectSegmenterGui
00048 {
00049 private:
00051   ros::NodeHandle root_nh_;
00053   ros::NodeHandle priv_nh_;
00054 
00056   ros::Publisher marker_pub_;
00057   
00058   ros::ServiceServer os_srv_;
00059 
00060   tf::TransformListener listener_;
00061 
00062   actionlib::SimpleActionClient<object_segmentation_gui::ObjectSegmentationGuiAction> *os_gui_action_client_;
00063 
00065   int num_markers_published_;
00067   int current_marker_id_;
00068 
00069   
00070   //------------------ Callbacks -------------------
00071 
00073   bool segmServiceCallback(tabletop_object_detector::TabletopSegmentation::Request &request,
00074                            tabletop_object_detector::TabletopSegmentation::Response &response);
00075 
00076   //------------------ Individual processing steps -------
00077   bool assembleSensorData(object_segmentation_gui::ObjectSegmentationGuiGoal &goal,
00078                           ros::Duration time_out);
00079 
00081   /*
00082     template <class PointCloudType>
00083     Table getTable(roslib::Header cloud_header, const tf::Transform &table_plane_trans,
00084     const PointCloudType &table_points);
00085   */
00086   
00088   /*
00089     template <class PointCloudType>
00090     void publishClusterMarkers(const std::vector<PointCloudType> &clusters, roslib::Header cloud_header);
00091   */
00092 
00093   //------------------- Complete processing -----
00094 
00096   /*
00097     void labelCloud(const sensor_msgs::PointCloud2 &cloud, const sensor_msgs::Image &labels,
00098     tabletop_object_detector::TabletopSegmentation::Response &response);
00099   */
00100   
00102   void clearOldMarkers(std::string frame_id);
00103 
00104 
00105 public:
00106 
00107   TabletopObjectSegmenterGui() :
00108     root_nh_(""), priv_nh_("~")
00109   {
00110     os_srv_ = root_nh_.advertiseService("/interactive_tabletop_segmentation",
00111                                         &TabletopObjectSegmenterGui::segmServiceCallback, this);
00112     
00113     os_gui_action_client_ = 0;
00114     
00115     ROS_INFO("Tabletop Object Segmenter Gui node started");
00116   }
00117 
00118   ~TabletopObjectSegmenterGui()
00119   {
00120     delete os_gui_action_client_;
00121   }
00122 
00123 };
00124 
00125 bool TabletopObjectSegmenterGui::segmServiceCallback(tabletop_object_detector::TabletopSegmentation::Request &request,
00126                                                      tabletop_object_detector::TabletopSegmentation::Response &response)
00127 {
00128   // reinitializing clients on each call to get around potential actionlib bug
00129   while(! os_gui_action_client_->waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
00130     {
00131       ROS_INFO("Waiting for action client on topic %s", "segmentation_popup");
00132     }
00133 
00134   if ( os_gui_action_client_ ) delete os_gui_action_client_;
00135   os_gui_action_client_ = new actionlib::SimpleActionClient<object_segmentation_gui::ObjectSegmentationGuiAction>
00136     ("segmentation_popup", true);
00137   
00138   object_segmentation_gui::ObjectSegmentationGuiGoal segm_goal;
00139   if (!assembleSensorData(segm_goal, ros::Duration(15.0))) return false;
00140   ROS_INFO("Assembled Sensor Data");
00141   os_gui_action_client_->sendGoal(segm_goal);
00142   ROS_INFO("Send Data as goal");
00143   while (!os_gui_action_client_->waitForResult(ros::Duration(0.5)) && priv_nh_.ok())
00144     {
00145       ROS_INFO("Waiting for result from action client on topic %s", "segmentation_popup");
00146     }
00147   if (!priv_nh_.ok()) 
00148     {
00149       os_gui_action_client_->cancelGoal();
00150       return false;
00151     }
00152   
00153   if (os_gui_action_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00154     {
00155       ROS_INFO("The segmentation gui action has not succeeded;");
00156       response.result = response.OTHER_ERROR;
00157     }
00158   else
00159     {    
00160       ROS_INFO("Segmentation Gui grasp has succeeded;");
00161       /*
00162       sensor_msgs::Image labeledSegm = os_gui_action_client_->getResult()->labeledSegments;
00163       
00164       labelCloud(segm_goal.sensor_data.point_cloud, labeledSegm, response);
00165       clearOldMarkers(segm_goal.sensor_data.point_cloud.header.frame_id);
00166       */
00167     }
00168   return true;
00169 }
00170 
00171 bool TabletopObjectSegmenterGui::assembleSensorData(object_segmentation_gui::ObjectSegmentationGuiGoal &goal,
00172                                                     ros::Duration time_out) 
00173 {
00174   sensor_msgs::Image::ConstPtr recent_image;
00175   stereo_msgs::DisparityImage::ConstPtr recent_disparity_image;
00176   sensor_msgs::CameraInfo::ConstPtr recent_camera_info;
00177   sensor_msgs::Image::ConstPtr recent_wide_image;
00178   sensor_msgs::CameraInfo::ConstPtr recent_wide_camera_info;
00179 
00180   ROS_INFO("Segmentation through User Interaction: waiting for messages...");
00181   std::string image_topic("/narrow_stereo/left/image_rect");
00182   std::string disparity_image_topic("/narrow_stereo_textured/disparity");
00183   std::string camera_info_topic("/narrow_stereo_textured/left/camera_info");
00184   std::string wide_camera_info_topic("/wide_stereo/left/camera_info");
00185   std::string wide_image_topic("/wide_stereo/left/image_rect_color");
00186 
00187   ros::Time start_time = ros::Time::now();
00188   while ((!recent_image || !recent_disparity_image || !recent_camera_info 
00189           || !recent_wide_image || !recent_wide_camera_info) && priv_nh_.ok())
00190     {
00191       if (!recent_image)
00192         {
00193           ROS_INFO_STREAM("  Waiting for message on topic " << image_topic);
00194             recent_image = ros::topic::waitForMessage<sensor_msgs::Image>(image_topic, root_nh_, ros::Duration(0.5));
00195         }
00196       if (!recent_disparity_image)
00197         {
00198           ROS_INFO_STREAM("  Waiting for message on topic " << disparity_image_topic);
00199           recent_disparity_image = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00200             (disparity_image_topic, root_nh_, ros::Duration(0.5));
00201         }
00202       if (!recent_camera_info)
00203         {
00204           ROS_INFO_STREAM("  Waiting for message on topic " << camera_info_topic);
00205           recent_camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00206             (camera_info_topic, root_nh_, ros::Duration(0.5));
00207         }
00208       if (!recent_wide_camera_info)
00209         {
00210           ROS_INFO_STREAM("  Waiting for message on topic " << wide_camera_info_topic);
00211           recent_wide_camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00212             (wide_camera_info_topic, root_nh_, ros::Duration(0.5));
00213         }
00214       if (!recent_wide_image)
00215         {
00216           ROS_INFO_STREAM("  Waiting for message on topic " << wide_image_topic);
00217           recent_wide_image = ros::topic::waitForMessage<sensor_msgs::Image>(wide_image_topic, root_nh_, 
00218                                                                              ros::Duration(0.5));
00219         }
00220       
00221       ros::Time current_time = ros::Time::now();
00222       if (time_out >= ros::Duration(0) && current_time - start_time >= time_out)
00223         {
00224           ROS_INFO("Timed out");
00225           return false;
00226         }
00227     }
00228   if (!priv_nh_.ok()) return false;
00229   ROS_INFO("All required messages received");
00230   
00231   goal.image = *recent_image;
00232   goal.disparity_image = *recent_disparity_image;
00233   goal.camera_info = *recent_camera_info;
00234   goal.wide_field = *recent_wide_image;
00235   goal.wide_camera_info = *recent_wide_camera_info;
00236 
00237   return true;
00238 }
00239  
00240 /*
00241 void TabletopObjectSegmenterGui::labelCloud(const sensor_msgs::PointCloud2 &cloud, 
00242                                             const sensor_msgs::Image &labels,
00243                                             tabletop_object_detector::TabletopSegmentation::Response &response)
00244 {
00245   // need association between pixels and point clouds 
00246   // make a table message -> see tabletop_segmentation processCloud()
00247   // assume background label=0, table label=1
00248   // assamble object clusters
00249   
00250 }
00251 */
00252 
00253 void TabletopObjectSegmenterGui::clearOldMarkers(std::string frame_id)
00254 {
00255   for (int id=current_marker_id_; id < num_markers_published_; id++)
00256     {
00257       visualization_msgs::Marker delete_marker;
00258       delete_marker.header.stamp = ros::Time::now();
00259       delete_marker.header.frame_id = frame_id;
00260       delete_marker.id = id;
00261       delete_marker.action = visualization_msgs::Marker::DELETE;
00262       delete_marker.ns = "tabletop_node";
00263       marker_pub_.publish(delete_marker);
00264     }
00265   num_markers_published_ = current_marker_id_;
00266   current_marker_id_ = 0;
00267 }
00268 
00269 int main(int argc, char **argv)
00270 {
00271   ros::init(argc, argv, "tabletop_segmentation_gui_node");
00272   TabletopObjectSegmenterGui node;
00273   ros::spin();
00274   return 0;
00275 }
tabletop_object_segmentation_gui_node.cpp
