$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 if (request.reset_static_map) collision_map_.resetStaticMap(); 00066 00067 //demo_synchronizer::getClient().sync(2, "Detecting table and objects"); 00068 //demo_synchronizer::getClient().rviz(1, "Narrow stereo textured;Collision models"); 00069 00070 //process the table, if requested 00071 tabletop_object_detector::Table table = request.detection_result.table; 00072 if (table.x_min!=0 || table.x_max!=0 || table.y_min!=0 || table.y_max!=0) 00073 { 00074 collision_map_.processCollisionGeometryForTable(table, "table"); 00075 response.collision_support_surface_name = "table"; 00076 } 00077 00078 //each cluster gets its own graspable object, regardless if the object detector 00079 //thinks it should have been merged with another cluster 00080 for (size_t c=0; c<request.detection_result.clusters.size(); c++) 00081 { 00082 object_manipulation_msgs::GraspableObject object; 00083 00084 //------------- process the cluster part ------------------- 00085 object.cluster = request.detection_result.clusters[c]; 00086 //convert the result to the desired frame 00087 if (!request.desired_frame.empty()) 00088 { 00089 00090 object.cluster.header.stamp = ros::Time(0); 00091 try 00092 { 00093 listener_.transformPointCloud(request.desired_frame, object.cluster, object.cluster); 00094 } 00095 catch (tf::TransformException ex) 00096 { 00097 ROS_ERROR("Failed to transform cluster to %s frame; exception: %s", 00098 request.desired_frame.c_str(), ex.what()); 00099 return false; 00100 } 00101 //the desired frame becomes the reference frame of the object 00102 object.reference_frame_id = request.desired_frame; 00103 } 00104 else 00105 { 00106 //the cluster reference frame becomes the reference frame of the object 00107 object.reference_frame_id = object.cluster.header.frame_id; 00108 } 00109 //compute the cluster bounding box 00110 object_manipulation_msgs::ClusterBoundingBox bbox; 00111 collision_map_.getClusterBoundingBox(object.cluster, bbox.pose_stamped, bbox.dimensions); 00112 //extend the bbox to the table along its z axis 00113 if ( !collision_map_.extendBoundingBoxZToTable(request.detection_result.table, 00114 bbox.pose_stamped, bbox.dimensions) ) 00115 { 00116 ROS_WARN("Failed to extend bbox to table; using original dimensions"); 00117 } 00118 00119 //-------------- add the object to the collision map as a bounding box --------------- 00120 std::string collision_name; 00121 collision_map_.processCollisionGeometryForBoundingBox(bbox, collision_name); 00122 //insert its collision name into the list 00123 response.collision_object_names.push_back(collision_name); 00124 00125 00126 /* 00127 //-------------- also provide the recognition information, if any -------------- 00128 //temporary: decide here if the object is recognized or not 00129 //TODO remove this 00130 00131 int ri = request.detection_result.cluster_model_indices[c]; 00132 ROS_WARN("1"); 00133 if (!request.detection_result.models[ri].model_list.empty() && 00134 request.detection_result.models[ri].model_list[0].confidence < 0.005) 00135 { 00136 object.potential_models = request.detection_result.models[ri].model_list; 00137 //convert the results to the desired frame 00138 ROS_WARN("2"); 00139 if (!request.desired_frame.empty()) 00140 { 00141 for (size_t m=0; m<object.potential_models.size(); m++) 00142 { 00143 ROS_WARN("%d",m); 00144 object.potential_models[m].pose.header.stamp = ros::Time(0); 00145 try 00146 { 00147 listener_.transformPose(request.desired_frame, 00148 object.potential_models[m].pose, object.potential_models[m].pose); 00149 } 00150 catch (tf::TransformException ex) 00151 { 00152 ROS_ERROR("Failed to transform pose or cluster to %s frame; exception: %s", 00153 request.desired_frame.c_str(), ex.what()); 00154 return false; 00155 } 00156 } 00157 } 00158 00159 } 00160 */ 00161 //----------------- add the object to the list ------------------------- 00162 response.graspable_objects.push_back(object); 00163 } 00164 00165 if (request.take_static_collision_map) 00166 { 00167 //demo_synchronizer::getClient().rviz(1, "Narrow stereo textured;Collision models;Tilt scan"); 00168 collision_map_.takeStaticMap(); 00169 //demo_synchronizer::getClient().rviz(1, "Narrow stereo textured;Collision models;Collision map"); 00170 } 00171 return true; 00172 } 00173 catch (CollisionMapException &ex) 00174 { 00175 ROS_ERROR("Collision map processing error; exception: %s", ex.what()); 00176 return false; 00177 } 00178 } 00179 00180 public: 00181 CollisionMapProcessor() : priv_nh_("~"), root_nh_("") 00182 { 00183 //wait forever for connections 00184 collision_map_.connectionsEstablished(ros::Duration(-1.0)); 00185 //advertise service 00186 processing_srv_ = priv_nh_.advertiseService(COLLISION_SERVICE_NAME, 00187 &CollisionMapProcessor::serviceCallback, this); 00188 } 00189 00190 ~CollisionMapProcessor() {} 00191 }; 00192 00193 } //namespace 00194 00195 int main(int argc, char **argv) 00196 { 00197 ros::init(argc, argv, "tabletop_collision_map_processing_node"); 00198 tabletop_collision_map_processing::CollisionMapProcessor node; 00199 00200 ros::spin(); 00201 return 0; 00202 }