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         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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


katana_tabletop_collision_map_processing
Author(s): Henning Deeken
autogenerated on Thu Jan 3 2013 14:41:11