$search
00001 /********************************************************************* 00002 * 00003 * Copyright (c) 2009, Willow Garage, Inc. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions 00008 * are met: 00009 * 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above 00013 * copyright notice, this list of conditions and the following 00014 * disclaimer in the documentation and/or other materials provided 00015 * with the distribution. 00016 * * Neither the name of the Willow Garage nor the names of its 00017 * contributors may be used to endorse or promote products derived 00018 * from this software without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 *********************************************************************/ 00033 00034 // Author(s): Matei Ciocarlie 00035 00036 #include <ros/ros.h> 00037 00038 #include <tf/transform_listener.h> 00039 00040 #include "tabletop_collision_map_processing/collision_map_interface.h" 00041 #include "tabletop_collision_map_processing/TabletopCollisionMapProcessing.h" 00042 00043 namespace tabletop_collision_map_processing { 00044 00045 static const std::string COLLISION_SERVICE_NAME = "tabletop_collision_map_processing"; 00046 00047 class CollisionMapProcessor 00048 { 00049 private: 00050 ros::NodeHandle priv_nh_; 00051 ros::NodeHandle root_nh_; 00052 CollisionMapInterface collision_map_; 00053 ros::ServiceServer processing_srv_; 00054 tf::TransformListener listener_; 00055 00056 bool serviceCallback(TabletopCollisionMapProcessing::Request &request, 00057 TabletopCollisionMapProcessing::Response &response) 00058 { 00059 00060 try 00061 { 00062 // perform resets, if requested 00063 if (request.reset_collision_models) collision_map_.resetCollisionModels(); 00064 if (request.reset_attached_models) collision_map_.resetAttachedModels(); 00065 00066 //process the table, if requested 00067 tabletop_object_detector::Table table = request.detection_result.table; 00068 if (table.x_min!=0 || table.x_max!=0 || table.y_min!=0 || table.y_max!=0) 00069 { 00070 collision_map_.processCollisionGeometryForTable(table, "table"); 00071 response.collision_support_surface_name = "table"; 00072 } 00073 00074 //each cluster gets its own graspable object, regardless if the object detector 00075 //thinks it should have been merged with another cluster 00076 for (size_t c=0; c<request.detection_result.clusters.size(); c++) 00077 { 00078 object_manipulation_msgs::GraspableObject object; 00079 00080 //------------- process the cluster part ------------------- 00081 object.cluster = request.detection_result.clusters[c]; 00082 //convert the result to the desired frame 00083 if (!request.desired_frame.empty()) 00084 { 00085 object.cluster.header.stamp = ros::Time(0); 00086 try 00087 { 00088 listener_.transformPointCloud(request.desired_frame, object.cluster, object.cluster); 00089 } 00090 catch (tf::TransformException ex) 00091 { 00092 ROS_ERROR("Failed to transform cluster to %s frame; exception: %s", 00093 request.desired_frame.c_str(), ex.what()); 00094 return false; 00095 } 00096 //the desired frame becomes the reference frame of the object 00097 object.reference_frame_id = request.desired_frame; 00098 } 00099 else 00100 { 00101 //the cluster reference frame becomes the reference frame of the object 00102 object.reference_frame_id = object.cluster.header.frame_id; 00103 } 00104 //compute the cluster bounding box 00105 object_manipulation_msgs::ClusterBoundingBox bbox; 00106 collision_map_.getClusterBoundingBox(object.cluster, bbox.pose_stamped, bbox.dimensions); 00107 //extend the bbox to the table along its z axis 00108 if ( !collision_map_.extendBoundingBoxZToTable(request.detection_result.table, 00109 bbox.pose_stamped, bbox.dimensions) ) 00110 { 00111 ROS_WARN("Failed to extend bbox to table; using original dimensions"); 00112 } 00113 00114 //-------------- add the object to the collision map as a bounding box --------------- 00115 std::string collision_name; 00116 collision_map_.processCollisionGeometryForBoundingBox(bbox, collision_name); 00117 //insert its collision name into the list 00118 response.collision_object_names.push_back(collision_name); 00119 00120 //-------------- also provide the recognition information, if any -------------- 00121 //temporary: decide here if the object is recognized or not 00122 //TODO remove this 00123 int ri = request.detection_result.cluster_model_indices[c]; 00124 if (!request.detection_result.models[ri].model_list.empty() && 00125 request.detection_result.models[ri].model_list[0].confidence < 0.005) 00126 { 00127 object.potential_models = request.detection_result.models[ri].model_list; 00128 //convert the results to the desired frame 00129 if (!request.desired_frame.empty()) 00130 { 00131 for (size_t m=0; m<object.potential_models.size(); m++) 00132 { 00133 object.potential_models[m].pose.header.stamp = ros::Time(0); 00134 try 00135 { 00136 listener_.transformPose(request.desired_frame, 00137 object.potential_models[m].pose, object.potential_models[m].pose); 00138 } 00139 catch (tf::TransformException ex) 00140 { 00141 ROS_ERROR("Failed to transform pose or cluster to %s frame; exception: %s", 00142 request.desired_frame.c_str(), ex.what()); 00143 return false; 00144 } 00145 } 00146 } 00147 } 00148 00149 //----------------- add the object to the list ------------------------- 00150 response.graspable_objects.push_back(object); 00151 } 00152 00153 return true; 00154 } 00155 catch (CollisionMapException &ex) 00156 { 00157 ROS_ERROR("Collision map processing error; exception: %s", ex.what()); 00158 return false; 00159 } 00160 } 00161 00162 public: 00163 CollisionMapProcessor() : priv_nh_("~"), root_nh_("") 00164 { 00165 //wait forever for connections 00166 collision_map_.connectionsEstablished(ros::Duration(-1.0)); 00167 //advertise service 00168 processing_srv_ = priv_nh_.advertiseService(COLLISION_SERVICE_NAME, 00169 &CollisionMapProcessor::serviceCallback, this); 00170 } 00171 00172 ~CollisionMapProcessor() {} 00173 }; 00174 00175 } //namespace 00176 00177 int main(int argc, char **argv) 00178 { 00179 ros::init(argc, argv, "tabletop_collision_map_processing_node"); 00180 tabletop_collision_map_processing::CollisionMapProcessor node; 00181 ros::spin(); 00182 return 0; 00183 }