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 <interactive_perception_msgs/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 #include <tabletop_object_detector/Table.h> 00043 00044 #include <tabletop_collision_map_processing/collision_map_interface.h> 00045 00046 #include <object_manipulator/tools/mechanism_interface.h> 00047 00048 #include <pr2_interactive_object_detection/UserCommandAction.h> 00049 #include <pr2_interactive_object_detection/msg_saver.h> 00050 00051 #include <stereo_msgs/DisparityImage.h> 00052 #include <sensor_msgs/Image.h> 00053 #include <sensor_msgs/PointCloud2.h> 00054 #include <sensor_msgs/CameraInfo.h> 00055 #include <visualization_msgs/Marker.h> 00056 00058 class InteractiveObjDetBackend 00059 { 00060 public: 00061 00062 InteractiveObjDetBackend(); 00063 ~InteractiveObjDetBackend(); 00064 00065 private: 00066 00068 void processUserCommand(const pr2_interactive_object_detection::UserCommandGoal::ConstPtr &goal); 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 void resetMarkers( ); 00102 00104 int printObjects(const std::vector<manipulation_msgs::GraspableObject> &objects); 00105 00107 bool getModelInfo(const household_objects_database_msgs::DatabaseModelPose &model_pose, 00108 std::string &name, std::string &all_tags); 00109 00111 bool getPose( geometry_msgs::Pose &pose, std::string from_frame, std::string to_frame ); 00112 00114 bool transformPose( geometry_msgs::PoseStamped &pose, std::string target_frame ); 00115 00117 bool getModelMesh( int model_id, shape_msgs::Mesh& mesh ); 00118 00120 bool addTableToCollisionMap(tabletop_object_detector::Table table ); 00121 00122 void statusFeedback( std::string status ); 00123 void abortAction( std::string error ); 00124 void finishAction( std::string result ); 00125 void preemptAction( ); 00126 00128 template <class ServiceType> 00129 bool connectService( ros::ServiceClient& service_client, std::string topic ); 00130 00131 void publishFitMarkers( 00132 const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models, 00133 const tabletop_object_detector::Table &table ); 00134 00135 void clearOldMarkers(std::string frame_id); 00136 00137 ros::NodeHandle root_nh_; 00138 ros::NodeHandle priv_nh_; 00139 00140 ros::ServiceClient rgbd_assembler_client_; 00141 00142 ros::ServiceClient get_model_description_client_; 00143 ros::ServiceClient get_model_mesh_client_; 00144 00145 ros::ServiceClient auto_seg_client_; 00146 00147 ros::ServiceClient auto_obj_rec_client_; 00148 00149 ros::ServiceClient tabletop_detection_client_; 00150 00152 ros::Publisher marker_pub_; 00153 00154 // requests a popup where the user refines detection results 00155 actionlib::SimpleActionClient<interactive_perception_msgs::ObjectRecognitionGuiAction> *obj_rec_popup_client_; 00156 00157 // receives commands from the gui control interface 00158 actionlib::SimpleActionServer<pr2_interactive_object_detection::UserCommandAction> *user_command_server_; 00159 00160 // collection of the sensor data 00161 sensor_msgs::Image image_; 00162 stereo_msgs::DisparityImage disparity_image_; 00163 sensor_msgs::CameraInfo camera_info_; 00164 sensor_msgs::PointCloud2 point_cloud_; 00165 00166 rgbd_assembler::RgbdAssembly rgbd_assembler_srv_; 00167 00168 // requests & results of intermediate steps 00169 tabletop_object_detector::TabletopSegmentationResponse segmentation_result_; 00170 tabletop_object_detector::TabletopObjectRecognitionResponse recognition_result_; 00171 00172 tf::TransformListener tf_listener_; 00173 00174 // used for publishing the result message 00175 ros::Publisher result_publisher_; 00176 00177 // for adding the table to the collision map 00178 tabletop_collision_map_processing::CollisionMapInterface collision_map_interface_; 00179 00181 int current_marker_id_; 00182 00183 std::vector< visualization_msgs::Marker > delete_markers_; 00184 00185 // if we did interactive segmentation in the first step, we will treat the result differently 00186 bool segmentation_was_interactive_; 00187 00188 // parameters 00189 double min_marker_quality_; 00190 00191 std::string robot_reference_frame_id_; 00192 00193 double table_x_; 00194 double table_y_; 00195 double table_z_; 00196 }; 00197 00198 #endif