tabletop_collision_map_processing_node.cpp
Go to the documentation of this file.
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 }


tabletop_collision_map_processing
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:49:29