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 #ifndef InteractiveObjRecBackend_H 00031 #define InteractiveObjRecBackend_H 00032 00033 #include <actionlib/client/simple_action_client.h> 00034 #include <actionlib/server/simple_action_server.h> 00035 00036 #include <object_recognition_gui/ObjectRecognitionGuiAction.h> 00037 00038 #include <rgbd_assembler/RgbdAssembly.h> 00039 00040 #include <tabletop_object_detector/TabletopSegmentation.h> 00041 #include <tabletop_object_detector/TabletopObjectRecognition.h> 00042 00043 #include <object_manipulator/tools/mechanism_interface.h> 00044 00045 #include <pr2_interactive_object_detection/UserCommandAction.h> 00046 #include <pr2_interactive_object_detection/msg_saver.h> 00047 00048 #include <stereo_msgs/DisparityImage.h> 00049 #include <sensor_msgs/Image.h> 00050 #include <sensor_msgs/PointCloud2.h> 00051 #include <sensor_msgs/CameraInfo.h> 00052 #include <visualization_msgs/Marker.h> 00053 00055 class InteractiveObjDetBackend 00056 { 00057 public: 00058 00059 InteractiveObjDetBackend(); 00060 ~InteractiveObjDetBackend(); 00061 00062 private: 00063 00065 void processUserCommand(const pr2_interactive_object_detection::UserCommandGoal::ConstPtr &goal); 00066 00068 bool lookAtTable( ); 00069 00071 bool getSensorData( ros::Duration time_out ); 00072 00074 bool doSegmentation( bool interactive ); 00075 00077 bool doInteractiveSegmentation( ); 00078 00080 bool doAutoSegmentation( ); 00081 00083 bool doRecognition( bool interactive ); 00084 00086 bool doAutoRecognition( ); 00087 00089 bool doInteractiveRecognition(); 00090 00092 bool doAutoDetection( ); 00093 00095 bool doDetection( bool interactive ); 00096 00098 bool publishResult( int &num_recognized ); 00099 00101 int printObjects(const std::vector<object_manipulation_msgs::GraspableObject> &objects); 00102 00104 bool getModelInfo(const household_objects_database_msgs::DatabaseModelPose &model_pose, 00105 std::string &name, std::string &all_tags); 00106 00108 bool getPose( geometry_msgs::Pose &pose, std::string from_frame, std::string to_frame ); 00109 00111 bool transformPose( geometry_msgs::PoseStamped &pose, std::string target_frame ); 00112 00114 bool getModelMesh( int model_id, geometric_shapes_msgs::Shape& mesh ); 00115 00116 void statusFeedback( std::string status ); 00117 void abortAction( std::string error ); 00118 void finishAction( std::string result ); 00119 void preemptAction( ); 00120 00122 template <class ServiceType> 00123 bool connectService( ros::ServiceClient& service_client, std::string topic ); 00124 00125 void publishFitMarkers( 00126 const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models, 00127 const tabletop_object_detector::Table &table ); 00128 00129 void clearOldMarkers(std::string frame_id); 00130 00131 ros::NodeHandle root_nh_; 00132 ros::NodeHandle priv_nh_; 00133 00134 ros::ServiceClient rgbd_assembler_client_; 00135 00136 ros::ServiceClient get_model_description_client_; 00137 ros::ServiceClient get_model_mesh_client_; 00138 00139 ros::ServiceClient auto_seg_client_; 00140 00141 ros::ServiceClient auto_obj_rec_client_; 00142 00143 ros::ServiceClient tabletop_detection_client_; 00144 00146 ros::Publisher marker_pub_; 00147 00148 // requests a popup where the user refines detection results 00149 actionlib::SimpleActionClient<object_recognition_gui::ObjectRecognitionGuiAction> *obj_rec_popup_client_; 00150 00151 // receives commands from the gui control interface 00152 actionlib::SimpleActionServer<pr2_interactive_object_detection::UserCommandAction> *user_command_server_; 00153 00154 // collection of the sensor data 00155 sensor_msgs::Image image_; 00156 stereo_msgs::DisparityImage disparity_image_; 00157 sensor_msgs::CameraInfo camera_info_; 00158 sensor_msgs::PointCloud2 point_cloud_; 00159 00160 rgbd_assembler::RgbdAssembly rgbd_assembler_srv_; 00161 00162 // requests & results of intermediate steps 00163 tabletop_object_detector::TabletopSegmentationResponse segmentation_result_; 00164 tabletop_object_detector::TabletopObjectRecognitionResponse recognition_result_; 00165 00166 // used for moving the head 00167 object_manipulator::MechanismInterface mech_interface_; 00168 00169 tf::TransformListener tf_listener_; 00170 00171 // used for publishing the result message 00172 ros::Publisher result_publisher_; 00173 00175 int current_marker_id_; 00176 00177 std::vector< visualization_msgs::Marker > delete_markers_; 00178 00179 // if we did interactive segmentation in the first step, we will treat the result differently 00180 bool segmentation_was_interactive_; 00181 00182 // parameters 00183 double min_marker_quality_; 00184 00185 std::string robot_reference_frame_id_; 00186 00187 double table_x_; 00188 double table_y_; 00189 double table_z_; 00190 }; 00191 00192 #endif