katana_tabletop_collision_map_processing: katana_tabletop_collision_map_processing: collision_map

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): Gil Jones and Matei Ciocarlie
00035 
00036 #include "tabletop_collision_map_processing/collision_map_interface.h"
00037 
00038 #include <tf/transform_datatypes.h>
00039 
00040 #include <std_srvs/Empty.h>
00041 
00042 #include <mapping_msgs/CollisionObject.h>
00043 #include <mapping_msgs/AttachedCollisionObject.h>
00044 
00045 #include <object_manipulation_msgs/FindClusterBoundingBox.h>
00046 
00047 #include <household_objects_database_msgs/DatabaseModelPose.h>
00048 #include <household_objects_database_msgs/GetModelMesh.h>
00049 
00050 namespace tabletop_collision_map_processing {
00051 
00052 static const std::string RESET_COLLISION_MAP_NAME = "collision_map_self_occ_node/reset";
00053 static const std::string MAKE_STATIC_COLLISION_MAP_ACTION_NAME = "make_static_collision_map";
00054 static const std::string CLUSTER_BOUNDING_BOX_NAME = "find_cluster_bounding_box";
00055 
00056 CollisionMapInterface::CollisionMapInterface() :
00057   root_nh_(""),
00058   priv_nh_("~"),
00059   connections_established_(false),
00060   collision_object_current_id_(0),
00061   make_static_collision_map_client_(root_nh_, MAKE_STATIC_COLLISION_MAP_ACTION_NAME, true)
00062 {
00063   collision_object_pub_ = root_nh_.advertise<mapping_msgs::CollisionObject>("collision_object", 10);
00064   attached_object_pub_ = root_nh_.advertise<mapping_msgs::AttachedCollisionObject>("attached_collision_object", 10);
00065 }
00066 
00067 void CollisionMapInterface::connectServices()
00068 {
00069   collision_map_reset_client_ = root_nh_.serviceClient<std_srvs::Empty>(RESET_COLLISION_MAP_NAME, true);
00070   cluster_bounding_box_client_ = root_nh_.serviceClient<object_manipulation_msgs::FindClusterBoundingBox>
00071     (CLUSTER_BOUNDING_BOX_NAME, true);
00072   priv_nh_.param<std::string>("static_map_cloud_name", static_map_cloud_name_, "full_cloud_filtered");
00073   priv_nh_.param<int>("point_skip_num", point_skip_num_, 1);
00074   priv_nh_.param<int>("collision_box_size", collision_box_size_, 0.003);
00075 }
00076 
00077 bool CollisionMapInterface::connectionsPresent()
00078 {
00079   if ( !ros::service::exists(RESET_COLLISION_MAP_NAME, true) ) return false;
00080   if ( !ros::service::exists(CLUSTER_BOUNDING_BOX_NAME, true) ) return false;
00081   if (!make_static_collision_map_client_.waitForServer(ros::Duration(0.1)) )
00082   {
00083     ROS_INFO("Make static collision map action server is not connected");
00084     return false;
00085   }
00086   return true;
00087 }
00088 
00089 bool CollisionMapInterface::connectionsEstablished(ros::Duration timeout)
00090 {
00091   if (connections_established_) return true;
00092   ros::Time start_time = ros::Time::now();
00093   ros::Duration ping_time = ros::Duration(1.0);
00094   if (timeout >= ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00095   while (!connectionsPresent())
00096   {
00097     ping_time.sleep();
00098     if (!root_nh_.ok()) return false;
00099     if (timeout >= ros::Duration(0) &&
00100         ros::Time::now() - start_time >= timeout) return false;
00101   }
00102   connectServices();
00103   connections_established_ = true;
00104   return true;
00105 }
00106 
00107 void CollisionMapInterface::resetCollisionModels()
00108 {
00109   ROS_WARN("CollisionMapInterface::resetCollisionModels"); // to be deleted
00110   mapping_msgs::CollisionObject reset_object;
00111   reset_object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00112   reset_object.header.frame_id = "katana_base_link";
00113   reset_object.header.stamp = ros::Time::now();
00114   reset_object.id = "all";
00115   collision_object_pub_.publish(reset_object);
00116   collision_object_current_id_ = 0;
00117 
00118 }
00119 
00120 void CollisionMapInterface::resetAttachedModels()
00121 {
00122   ROS_WARN("CollisionMapInterface::resetAttachedModels"); // to be deleted
00123   mapping_msgs::CollisionObject reset_object;
00124   reset_object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00125   reset_object.header.frame_id = "katana_base_link";
00126   reset_object.header.stamp = ros::Time::now();
00127   reset_object.id = "all";
00128   mapping_msgs::AttachedCollisionObject reset_attached_objects;
00129   reset_attached_objects.object.header.frame_id = "katana_base_link";
00130   reset_attached_objects.object.header.stamp = ros::Time::now();
00131   reset_attached_objects.link_name = "all";
00132   reset_attached_objects.object = reset_object;
00133   attached_object_pub_.publish(reset_attached_objects);
00134 }
00135 
00136 void CollisionMapInterface::resetStaticMap()
00137 {
00138   ROS_WARN("CollisionMapInterface::resetStaticMap"); // to be deleted
00139   std_srvs::Empty srv;
00140   if (!collision_map_reset_client_.call(srv))
00141   {
00142     ROS_ERROR("Collision map reset call failed");
00143     throw CollisionMapException("reset failed");
00144   }
00145 }
00146 
00147 std::string CollisionMapInterface::getNextObjectName()
00148 {
00149   ROS_WARN("CollisionMapInterface::getNextObjectName"); // to be deleted
00150   std::ostringstream iss;
00151   iss << collision_object_current_id_++;
00152   if (collision_object_current_id_ > 10000) collision_object_current_id_ = 0;
00153   return "graspable_object_" + iss.str();
00154 }
00155 
00156 void CollisionMapInterface::takeStaticMap()
00157 {
00158   ROS_WARN("CollisionMapInterface::takeStaticMap"); // to be deleted
00159   collision_environment_msgs::MakeStaticCollisionMapGoal static_map_goal;
00160   static_map_goal.cloud_source = static_map_cloud_name_;
00161   static_map_goal.number_of_clouds = 2;
00162 
00163   make_static_collision_map_client_.sendGoal(static_map_goal);
00164 
00165   if ( !make_static_collision_map_client_.waitForResult(ros::Duration(30.0)) )
00166   {
00167     ROS_ERROR("Collision map was not formed in allowed time");
00168     throw CollisionMapException("static make was not formed in allowed time");
00169   }
00170 
00171   if(make_static_collision_map_client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00172   {
00173     ROS_ERROR("Some non-success state was reached for static collision map.  Proceed with caution");
00174     throw CollisionMapException(" static collision map failed");
00175   }
00176 }
00177 
00178 void CollisionMapInterface::processCollisionGeometryForTable(const tabletop_object_detector::Table &table,
00179                                                              std::string table_collision_name)
00180 {
00181   ROS_WARN("CollisionMapInterface::processCollisionGeometryForTable"); // to be deleted
00182   mapping_msgs::CollisionObject table_object;
00183   table_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00184   table_object.header.frame_id = table.pose.header.frame_id;
00185   table_object.header.stamp = ros::Time::now();
00186   table_object.shapes.resize(1);
00187   table_object.shapes[0].type = geometric_shapes_msgs::Shape::BOX;
00188   table_object.shapes[0].dimensions.resize(3);
00189   table_object.shapes[0].dimensions[0] = fabs(table.x_max-table.x_min);
00190   table_object.shapes[0].dimensions[1] = fabs(table.y_max-table.y_min);
00191   table_object.shapes[0].dimensions[2] = 0.01;
00192 
00193   ROS_INFO("Adding table with dimensions %f %f %f to collision map",
00194            table_object.shapes[0].dimensions[0], table_object.shapes[0].dimensions[1], table_object.shapes[0].dimensions[2]);
00195 
00196 
00197   //set the origin of the table object in the middle of the table
00198   tf::Transform table_trans;
00199   tf::poseMsgToTF(table.pose.pose, table_trans);
00200   tf::Transform table_translation;
00201   table_translation.setIdentity();
00202   table_translation.setOrigin( btVector3( (table.x_min + table.x_max)/2.0, (table.y_min + table.y_max)/2.0, 0.0) );
00203   table_trans = table_trans * table_translation;
00204   table_object.poses.resize(1);
00205   tf::poseTFToMsg(table_trans, table_object.poses[0]);
00206 
00207   table_object.id = table_collision_name;
00208   collision_object_pub_.publish(table_object);
00209 }
00210 
00211 void CollisionMapInterface::processCollisionGeometryForObject
00212   (const household_objects_database_msgs::DatabaseModelPose &model_pose,
00213    std::string &collision_name)
00214 {
00215   ROS_WARN("CollisionMapInterface::processCollisionGeometryForObject"); // to be deleted
00216   mapping_msgs::CollisionObject collision_object;
00217   collision_object.shapes.resize(1);
00218 
00219   if (!getMeshFromDatabasePose(model_pose, collision_object.shapes[0]))
00220   {
00221     throw CollisionMapException("Loading mesh for database object failed");
00222   }
00223   collision_object.header.frame_id = model_pose.pose.header.frame_id;
00224   collision_object.header.stamp = ros::Time::now();
00225   collision_object.poses.push_back(model_pose.pose.pose);
00226 
00227   collision_object.shapes[0].type = geometric_shapes_msgs::Shape::MESH;
00228   collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00229   collision_name = getNextObjectName();
00230   collision_object.id = collision_name;
00231   collision_object_pub_.publish(collision_object);
00232 }
00233 
00234 void
00235 CollisionMapInterface::processCollisionGeometryForBoundingBox(const object_manipulation_msgs::ClusterBoundingBox &box,
00236                                                               std::string &collision_name)
00237 {
00238   ROS_WARN("CollisionMapInterface::processCollisionGeometryForBoundingBox"); // to be deleted
00239   ROS_INFO("Adding bounding box with dimensions %f %f %f to collision map",
00240            box.dimensions.x, box.dimensions.y, box.dimensions.z);
00241 
00242   mapping_msgs::CollisionObject collision_object;
00243   collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00244   collision_name = getNextObjectName();
00245   collision_object.id = collision_name;
00246 
00247   collision_object.header.frame_id = "katana_base_link"; //box.pose_stamped.header.frame_id;
00248   collision_object.header.stamp = ros::Time::now();
00249 
00250   geometric_shapes_msgs::Shape shape;
00251   shape.type = geometric_shapes_msgs::Shape::BOX;
00252   shape.dimensions.resize(3);
00253   shape.dimensions[0] = box.dimensions.x;
00254   shape.dimensions[1] = box.dimensions.y;
00255   shape.dimensions[2] = box.dimensions.z;
00256   collision_object.shapes.push_back(shape);
00257   collision_object.poses.push_back(box.pose_stamped.pose);
00258   ROS_WARN("so long "); // to be deleted
00259   collision_object_pub_.publish(collision_object);
00260 }
00261 
00262 void CollisionMapInterface::processCollisionGeometryForCluster(const sensor_msgs::PointCloud &cluster,
00263                                                                std::string &collision_name, float collision_size)
00264 {
00265   ROS_WARN("CollisionMapInterface::processCollisionGeometryForCluster"); // to be deleted
00266   ROS_INFO("Adding cluster with %u points to collision map", (unsigned int)cluster.points.size());
00267 
00268   mapping_msgs::CollisionObject many_boxes;
00269   many_boxes.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00270   many_boxes.header = cluster.header;
00271   many_boxes.header.stamp = ros::Time::now();
00272   unsigned int num_to_use = (unsigned int)(cluster.points.size()/point_skip_num_);
00273   many_boxes.shapes.resize(num_to_use);
00274   many_boxes.poses.resize(num_to_use);
00275 
00276   if(collision_size < 0)
00277     collision_size = collision_box_size_;
00278 
00279   for(unsigned int i = 0; i < num_to_use; i++) {
00280     geometric_shapes_msgs::Shape shape;
00281     shape.type = geometric_shapes_msgs::Shape::BOX;
00282     shape.dimensions.resize(3);
00283     shape.dimensions[0] = collision_size;
00284     shape.dimensions[1] = collision_size;
00285     shape.dimensions[2] = collision_size;
00286     many_boxes.shapes[i]=shape;
00287     geometry_msgs::Pose pose;
00288     pose.position.x = cluster.points[i*point_skip_num_].x;
00289     pose.position.y = cluster.points[i*point_skip_num_].y;
00290     pose.position.z = cluster.points[i*point_skip_num_].z;
00291     pose.orientation.x = 0;
00292     pose.orientation.y = 0;
00293     pose.orientation.z = 0;
00294     pose.orientation.w = 1;
00295     many_boxes.poses[i] = pose;
00296   }
00297 
00298   collision_name = getNextObjectName();
00299   //use this name for the collision map
00300   many_boxes.id = collision_name;
00301   collision_object_pub_.publish(many_boxes);
00302 }
00303 
00304 bool CollisionMapInterface::getMeshFromDatabasePose(const household_objects_database_msgs::DatabaseModelPose &model_pose,
00305                                                     geometric_shapes_msgs::Shape& mesh)
00306 {
00307 
00308   ROS_WARN("CollisionMapInterface::getMeshFromDatabasePose"); // to be deleted
00309 
00310   static bool service_initialized = false;
00311   if (!service_initialized)
00312   {
00313     std::string get_model_mesh_srv_name;
00314     priv_nh_.param<std::string>("get_model_mesh_srv", get_model_mesh_srv_name, "get_model_mesh_srv");
00315     while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && root_nh_.ok() )
00316     {
00317       ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
00318     }
00319     if (!root_nh_.ok()) return false;
00320     get_model_mesh_srv_ = root_nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
00321       (get_model_mesh_srv_name, true);
00322     service_initialized = true;
00323   }
00324   household_objects_database_msgs::GetModelMesh get_mesh;
00325   get_mesh.request.model_id = model_pose.model_id;
00326   if ( !get_model_mesh_srv_.call(get_mesh) ||
00327        get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS )
00328   {
00329     return false;
00330   }
00331   mesh = get_mesh.response.mesh;
00332   return true;
00333 }
00334 
00335 void CollisionMapInterface::getClusterBoundingBox(const sensor_msgs::PointCloud &cluster,
00336                                                   geometry_msgs::PoseStamped &pose_stamped,
00337                                                   geometry_msgs::Vector3 &dimensions)
00338 {
00339   ROS_WARN("CollisionMapInterface::getClusterBoundingBox"); // to be deleted
00340 
00341   object_manipulation_msgs::FindClusterBoundingBox srv;
00342   srv.request.cluster = cluster;
00343   if (!cluster_bounding_box_client_.call(srv.request, srv.response))
00344   {
00345     ROS_ERROR("Failed to call cluster bounding box client");
00346     throw CollisionMapException("Failed to call cluster bounding box client");
00347   }
00348   pose_stamped = srv.response.pose;
00349   dimensions = srv.response.box_dims;
00350   if (dimensions.x == 0.0 && dimensions.y == 0.0 && dimensions.z == 0.0)
00351   {
00352     ROS_ERROR("Cluster bounding box client returned an error (0.0 bounding box)");
00353     throw CollisionMapException("Bounding box computation failed");
00354   }
00355 }
00356 
00358 bool CollisionMapInterface::extendBoundingBoxZToTable(const tabletop_object_detector::Table &table,
00359                                                       geometry_msgs::PoseStamped &pose_stamped,
00360                                                       geometry_msgs::Vector3 &dimensions)
00361 {
00362 
00363   ROS_WARN("CollisionMapInterface::extendBoundingBoxZToTable"); // to be deleted
00364 
00365   if (table.pose.header.frame_id != pose_stamped.header.frame_id)
00366   {
00367     ROS_INFO("cannot extend bbox to table, they are not in the same frame");
00368     ROS_INFO("table frame: %s vs. pose_stamped frame: %s",table.pose.header.frame_id.c_str(),  pose_stamped.header.frame_id.c_str());
00369     return false;
00370   }
00371 
00372   //get the distance from the box to the table
00373   btVector3 table_location(table.pose.pose.position.x,
00374                            table.pose.pose.position.y,
00375                            table.pose.pose.position.z);
00376   btVector3 bbox_location(pose_stamped.pose.position.x,
00377                           pose_stamped.pose.position.y,
00378                           pose_stamped.pose.position.z);
00379   btVector3 table_to_bbox = bbox_location - table_location;
00380   btTransform table_trans;
00381   tf::poseMsgToTF(table.pose.pose, table_trans);
00382   btVector3 table_z = table_trans.getBasis().getColumn(2);
00383   ROS_INFO("Table z: %f %f %f in frame %s", table_z.getX(), table_z.getY(), table_z.getZ(), table.pose.header.frame_id.c_str());
00384   double box_to_table_distance = table_to_bbox.dot(table_z);
00385   ROS_INFO("Table distance: %f", box_to_table_distance);
00386 
00387   //now we actually make the assumptions that the z axes are close to each other
00388   //as doing an actual computation was just too hard
00389   btTransform bbox_trans;
00390   tf::poseMsgToTF(pose_stamped.pose, bbox_trans);
00391   btVector3 bbox_z = bbox_trans.getBasis().getColumn(2);
00392   if (fabs(bbox_z.dot(table_z)) < 0.9848) //10 degrees
00393   {
00394     ROS_INFO("cannot extend bbox to table; z axes do not coincide");
00395     return false;
00396   }
00397 
00398   double z_to_table = box_to_table_distance;
00399   ROS_INFO("z_to_table: %f", z_to_table);
00400   if ( z_to_table < 0.005 || z_to_table > 0.3)
00401   {
00402     ROS_INFO("cannot extend bbox to table; getting z equal to %f", z_to_table);
00403     return false;
00404   }
00405   ROS_INFO("Old z: %f", dimensions.z);
00406   double new_z = dimensions.z/2.0 + z_to_table;
00407   ROS_INFO("New z: %f", new_z);
00408   pose_stamped.pose.position.z = pose_stamped.pose.position.z + (dimensions.z/2.0) - (new_z/2.0);
00409   dimensions.z = new_z;
00410   return true;
00411 }
00412 
00413 
00414 } //namespace tabletop_collision_map_processing
collision_map_interface.cpp
