collision_proximity_space.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <visualization_msgs/Marker.h>
00038 #include <visualization_msgs/MarkerArray.h>
00039 #include <planning_environment/models/model_utils.h>
00040 #include <collision_proximity/collision_proximity_space.h>
00041 #include <tf/tf.h>
00042 
00043 using collision_proximity::CollisionProximitySpace;
00044 
00045 // static double gen_rand(double min, double max)
00046 // {
00047 //   int rand_num = rand()%100+1;
00048 //   double result = min + (double)((max-min)*rand_num)/101.0;
00049 //   return result;
00050 // }
00051 
00052 static std::string makeStringFromUnsignedInt(unsigned int j)
00053 {
00054   std::stringstream ss;
00055   ss << j;
00056   return ss.str();
00057 }
00058 
00059 static std::string makeAttachedObjectId(std::string link, std::string object) 
00060 {
00061   return link+"_"+object;
00062 }
00063 
00064 CollisionProximitySpace::CollisionProximitySpace(const std::string& robot_description_name,
00065                                                  bool register_with_environment_server, bool use_signed_environment_field , bool use_signed_self_field) :
00066   priv_handle_("~")
00067 {
00068   collision_models_interface_ = new planning_environment::CollisionModelsInterface(robot_description_name,
00069                                                                                    register_with_environment_server);
00070 
00071   priv_handle_.param("size_x", size_x_, 3.0);
00072   priv_handle_.param("size_y", size_y_, 3.0);
00073   priv_handle_.param("size_z", size_z_, 4.0);
00074   priv_handle_.param("origin_x", origin_x_, -1.0);
00075   priv_handle_.param("origin_y", origin_y_, -1.5);
00076   priv_handle_.param("origin_z", origin_z_, -2.0);
00077   priv_handle_.param("resolution", resolution_, 0.02);
00078   priv_handle_.param("collision_tolerance", tolerance_, 0.00);
00079   priv_handle_.param("max_environment_distance", max_environment_distance_, 0.25);
00080   priv_handle_.param("max_self_distance", max_self_distance_, 0.1);
00081   priv_handle_.param("undefined_distance", undefined_distance_, 1.0);
00082 
00083   vis_distance_field_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
00084   vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("collision_proximity_body_spheres", 128);
00085   vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("collision_proximity_body_spheres_array", 128);
00086 
00087   if(use_signed_self_field)
00088   {
00089     self_distance_field_ = (distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>*)(new distance_field::SignedPropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_self_distance_));
00090   }
00091   else
00092   {
00093     self_distance_field_ = new distance_field::PropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_self_distance_);
00094   }
00095   if(use_signed_environment_field)
00096   {
00097     environment_distance_field_ = (distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>*)(new distance_field::SignedPropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_environment_distance_));
00098   }
00099   else
00100   {
00101     environment_distance_field_ = new distance_field::PropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_environment_distance_);
00102   }
00103 
00104   collision_models_interface_->addSetPlanningSceneCallback(boost::bind(&CollisionProximitySpace::setPlanningSceneCallback, this, _1));
00105   collision_models_interface_->addRevertPlanningSceneCallback(boost::bind(&CollisionProximitySpace::revertPlanningSceneCallback, this));
00106 
00107   loadRobotBodyDecompositions();
00108 
00109   loadDefaultCollisionOperations();
00110 
00111   /*
00112     shapes::Box base_box(1.0,1.0,.3);
00113     BodyDecomposition* bd = new BodyDecomposition("base_box", &base_box, resolution_);
00114     tf::Transform trans;
00115     trans.setIdentity();
00116     trans.setOrigin(tf::Vector3(0.0,0.0,.15));
00117     bd->updatePose(trans);
00118     BodyDecompositionVector* bdv = new BodyDecompositionVector();
00119     bdv->addToVector(bd);
00120     static_object_map_["base_box"] = bdv;
00121   */
00122 }
00123 
00124 CollisionProximitySpace::~CollisionProximitySpace()
00125 {
00126   delete collision_models_interface_;
00127   delete self_distance_field_;
00128   delete environment_distance_field_;
00129   for(std::map<std::string, BodyDecomposition*>::iterator it = body_decomposition_map_.begin();
00130       it != body_decomposition_map_.end();
00131       it++) {
00132     delete it->second;
00133   }
00134   deleteAllStaticObjectDecompositions();
00135   deleteAllAttachedObjectDecompositions();
00136 }
00137 
00138 void CollisionProximitySpace::deleteAllStaticObjectDecompositions()
00139 {
00140   for(std::map<std::string, BodyDecompositionVector*>::iterator it = static_object_map_.begin();
00141       it != static_object_map_.end();
00142       it++) {
00143     delete it->second;
00144   }
00145   static_object_map_.clear();
00146 }
00147 
00148 void CollisionProximitySpace::deleteAllAttachedObjectDecompositions()
00149 {
00150   for(std::map<std::string, BodyDecompositionVector*>::iterator it = attached_object_map_.begin();
00151       it != attached_object_map_.end();
00152       it++) {
00153     delete it->second;
00154   }
00155   attached_object_map_.clear();
00156   current_attached_body_decompositions_.clear();
00157   current_attached_body_names_.clear();
00158   current_attached_body_indices_.clear();
00159 }
00160 
00161 void CollisionProximitySpace::loadDefaultCollisionOperations()
00162 {
00163   std::map<std::string, bool> all_true_map;
00164   const planning_models::KinematicModel* kmodel = collision_models_interface_->getKinematicModel();
00165 
00166   for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) {
00167     all_true_map[kmodel->getLinkModels()[i]->getName()] = true;
00168   }
00169   
00170   for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) {
00171     intra_group_collision_links_[kmodel->getLinkModels()[i]->getName()] = all_true_map;
00172   }
00173 
00174   //creating lists for each group in planning groups
00175   const std::map<std::string, planning_models::KinematicModel::JointModelGroup*>& jmgm = 
00176     collision_models_interface_->getKinematicModel()->getJointModelGroupMap();
00177   for(std::map<std::string, planning_models::KinematicModel::JointModelGroup*>::const_iterator it = jmgm.begin();
00178       it != jmgm.end();
00179       it++) {
00180     enabled_self_collision_links_[it->first] = all_true_map;
00181     std::vector<std::string> link_names = it->second->getUpdatedLinkModelNames();
00182     for(unsigned int i = 0; i < link_names.size(); i++) {
00183       enabled_self_collision_links_[it->first][link_names[i]] = false;
00184     }
00185   }
00186 
00187   XmlRpc::XmlRpcValue coll_ops;
00188   if(!root_handle_.hasParam("/robot_description_planning/distance_collision_operations")) {
00189     ROS_WARN("No distance collision operations specified");
00190     return;
00191   }
00192   root_handle_.getParam("/robot_description_planning/distance_collision_operations", coll_ops);
00193   
00194   if(coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00195     ROS_WARN("distance_collision_operations is not an array");
00196     return;
00197   } 
00198   
00199   if(coll_ops.size() == 0) {
00200     ROS_WARN("No collision operations in distance collision operations");
00201     return;
00202   }
00203   
00204   for(int i = 0; i < coll_ops.size(); i++) {
00205     if(!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation")) {
00206       ROS_WARN("All collision operations must have two objects and an operation");
00207       continue;
00208     }
00209     std::string object1 = std::string(coll_ops[i]["object1"]);
00210     std::string object2 = std::string(coll_ops[i]["object2"]);
00211     std::string operation = std::string(coll_ops[i]["operation"]);
00212       
00213     if(jmgm.find(object1) == jmgm.end()) {
00214       if(object1 == object2) {
00215         //using for disabling self checks
00216         self_excludes_[object1] = true;
00217       } else {
00218         //must be intra_collision
00219         if(jmgm.find(object2) == jmgm.end()) {
00220           intra_group_collision_links_[object1][object2] = (operation == "enable");
00221           intra_group_collision_links_[object2][object1] = (operation == "enable");
00222         } else {
00223           std::vector<std::string> group_links = jmgm.find(object2)->second->getUpdatedLinkModelNames();
00224           for(unsigned int j = 0; j < group_links.size(); j++) {
00225             intra_group_collision_links_[object1][group_links[j]] = (operation == "enable");
00226             intra_group_collision_links_[group_links[j]][object1] = (operation == "enable");
00227           }
00228         }
00229       }
00230     } else if(jmgm.find(object2) == jmgm.end()) {
00231       std::vector<std::string> group_links = jmgm.find(object1)->second->getUpdatedLinkModelNames();
00232       for(unsigned int j = 0; j < group_links.size(); j++) {
00233         intra_group_collision_links_[object2][group_links[j]] = (operation == "enable");
00234         intra_group_collision_links_[group_links[j]][object2] = (operation == "enable");
00235       }
00236     } else {
00237       std::vector<std::string> group_links_1 = jmgm.find(object1)->second->getUpdatedLinkModelNames();
00238       std::vector<std::string> group_links_2 = jmgm.find(object2)->second->getUpdatedLinkModelNames();
00239       for(unsigned int j = 0; j < group_links_1.size(); j++) {
00240         for(unsigned int k = 0; k < group_links_2.size(); k++) {
00241           intra_group_collision_links_[group_links_1[j]][group_links_2[k]] = (operation == "enable");
00242           intra_group_collision_links_[group_links_2[k]][group_links_1[j]] = (operation == "enable");
00243         }
00244       }
00245     }
00246   }
00247 }
00248 
00249 void CollisionProximitySpace::loadRobotBodyDecompositions()
00250 {
00251   const planning_models::KinematicModel* kmodel = collision_models_interface_->getKinematicModel();
00252   
00253   for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) {
00254     if(kmodel->getLinkModels()[i]->getLinkShape() != NULL) {
00255       double padding = collision_models_interface_->getDefaultPadding();
00256 
00257       if(collision_models_interface_->getDefaultLinkPaddingMap().find(kmodel->getLinkModels()[i]->getName()) !=
00258           collision_models_interface_->getDefaultLinkPaddingMap().end())
00259       {
00260         padding = collision_models_interface_->getDefaultLinkPaddingMap().at(kmodel->getLinkModels()[i]->getName());
00261       }
00262 
00263       body_decomposition_map_[kmodel->getLinkModels()[i]->getName()] = new BodyDecomposition(kmodel->getLinkModels()[i]->getName(),
00264                                                                                              kmodel->getLinkModels()[i]->getLinkShape(),
00265                                                                                              resolution_/2.0, padding);
00266     }
00267   }
00268 }
00269 
00270 bool CollisionProximitySpace::setPlanningScene(const arm_navigation_msgs::PlanningScene& scene) {
00271   return(collision_models_interface_->setPlanningSceneWithCallbacks(scene));
00272 }
00273 
00274 void CollisionProximitySpace::setPlanningSceneCallback(const arm_navigation_msgs::PlanningScene& scene) 
00275 {
00276   ros::WallTime n1 = ros::WallTime::now();
00277   deleteAllStaticObjectDecompositions();
00278   deleteAllAttachedObjectDecompositions();
00279 
00280   syncObjectsWithCollisionSpace(*collision_models_interface_->getPlanningSceneState());
00281   
00282   prepareEnvironmentDistanceField(*collision_models_interface_->getPlanningSceneState());
00283   ros::WallTime n2 = ros::WallTime::now();
00284   ROS_DEBUG_STREAM("Setting environment took " << (n2-n1).toSec());
00285 }
00286 
00287 void CollisionProximitySpace::setupForGroupQueries(const std::string& group_name,
00288                                                    const arm_navigation_msgs::RobotState& rob_state,
00289                                                    std::vector<std::string>& link_names,
00290                                                    std::vector<std::string>& attached_body_names)
00291 {
00292   ros::WallTime n1 = ros::WallTime::now();
00293   //setting up current info
00294   current_group_name_ = group_name;
00295   //for trajectory safety check
00296   collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_name);
00297   planning_environment::setRobotStateAndComputeTransforms(rob_state,
00298                                                           *collision_models_interface_->getPlanningSceneState());
00299   getGroupLinkAndAttachedBodyNames(current_group_name_, 
00300                                    *collision_models_interface_->getPlanningSceneState(),
00301                                    current_link_names_, 
00302                                    current_link_indices_,
00303                                    current_attached_body_names_,
00304                                    current_attached_body_indices_);
00305   link_names = current_link_names_;
00306   attached_body_names = current_attached_body_names_;
00307   setupGradientStructures(current_link_names_,
00308                           current_attached_body_names_,
00309                           current_gradients_);
00310 
00311   current_link_body_decompositions_.clear();
00312   current_attached_body_decompositions_.clear();
00313   current_self_excludes_.clear();
00314   for(unsigned int i = 0; i < current_link_names_.size(); i++) {
00315     current_link_body_decompositions_.push_back(body_decomposition_map_[current_link_names_[i]]);
00316     if(self_excludes_.find(current_link_names_[i]) != self_excludes_.end()) {
00317       current_self_excludes_.push_back(false);
00318     } else {
00319       current_self_excludes_.push_back(true);
00320     }
00321   }
00322   for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) {
00323     current_attached_body_decompositions_.push_back(attached_object_map_[current_attached_body_names_[i]]);
00324   }
00325   current_intra_group_collision_links_.clear();
00326   unsigned int num_links = current_link_names_.size();
00327   unsigned int num_attached = current_attached_body_names_.size();
00328   unsigned int tot = num_links+num_attached;
00329   current_intra_group_collision_links_.resize(tot);
00330   for(unsigned int i = 0; i < tot; i++) {
00331     current_intra_group_collision_links_[i].resize(tot);
00332     for(unsigned int j = 0; j < tot; j++) {
00333       if(i < num_links && j < num_links) {
00334         current_intra_group_collision_links_[i][j] = intra_group_collision_links_.find(current_link_names_[i])->second.find(current_link_names_[j])->second;
00335         //ROS_INFO_STREAM("Setting intra_group_collision_links for " << current_link_names_[i] << " and " << current_link_names_[j] << " to " << current_intra_group_collision_links_[i][j]);
00336       } else {
00337         std::string name1;
00338         std::string name2;
00339         bool attached1 = false;
00340         bool attached2 = false;
00341         if(i < num_links) {
00342           name1 = current_link_names_[i];
00343         } else {
00344           attached1 = true;          
00345           name1 = current_attached_body_names_[i-num_links];
00346         }
00347         if(j < num_links) {
00348           name2 = current_link_names_[j];
00349         } else {
00350           attached2 = true;
00351           name2 = current_attached_body_names_[j-num_links];
00352         }
00353         current_intra_group_collision_links_[i][j] = true;
00354         if(attached1) {
00355           if(attached_object_collision_links_.find(name1)->second.find(name2) != attached_object_collision_links_.find(name1)->second.end() && !attached_object_collision_links_.find(name1)->second.find(name2)->second) {
00356             current_intra_group_collision_links_[i][j] = false;
00357           }
00358         }
00359         if(attached2) {
00360           if(attached_object_collision_links_.find(name2)->second.find(name1) != attached_object_collision_links_.find(name2)->second.end() && !attached_object_collision_links_.find(name2)->second.find(name1)->second) {
00361             current_intra_group_collision_links_[i][j] = false;
00362           }
00363         }
00364       }
00365     }
00366   }
00367   setBodyPosesGivenKinematicState(*collision_models_interface_->getPlanningSceneState());
00368   setDistanceFieldForGroupQueries(current_group_name_, *collision_models_interface_->getPlanningSceneState());
00369   ros::WallTime n2 = ros::WallTime::now();
00370   ROS_DEBUG_STREAM("Setting self for group " << current_group_name_ << " took " << (n2-n1).toSec());
00371   //visualizeDistanceField(self_distance_field_);
00372 }
00373 
00374 void CollisionProximitySpace::revertPlanningSceneCallback() {
00375 
00376   collision_models_interface_->bodiesLock();
00377   current_group_name_ = "";
00378 
00379   deleteAllStaticObjectDecompositions();
00380   deleteAllAttachedObjectDecompositions();
00381   collision_models_interface_->bodiesUnlock();
00382 }
00383 
00384 void CollisionProximitySpace::setCurrentGroupState(const planning_models::KinematicState& state)
00385 {
00386   ros::WallTime n1 = ros::WallTime::now();
00387   if(current_group_name_.empty()) {
00388     return;
00389   }
00390   tf::Transform inv = getInverseWorldTransform(state);
00391   for(unsigned int i = 0; i < current_link_indices_.size(); i++) {
00392     const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[current_link_indices_[i]];
00393     current_link_body_decompositions_[i]->updateSpheresPose(inv*ls->getGlobalCollisionBodyTransform());
00394   }
00395   for(unsigned int i = 0; i < current_attached_body_indices_.size(); i++) {
00396     const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[current_attached_body_indices_[i]];
00397     for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
00398       const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j];
00399       for(unsigned int k = 0; k < att_state->getGlobalCollisionBodyTransforms().size(); k++) {
00400         tf::Transform test = inv*att_state->getGlobalCollisionBodyTransforms()[k];
00401         current_attached_body_decompositions_[i]->updateSpheresPose(k, inv*att_state->getGlobalCollisionBodyTransforms()[k]);
00402       }
00403     }
00404   }
00405   updateSphereLocations(current_link_names_, current_attached_body_names_, current_gradients_);
00406   ROS_DEBUG_STREAM("Group state update took " << (ros::WallTime::now()-n1).toSec());
00407 }
00408 
00409 void CollisionProximitySpace::setBodyPosesGivenKinematicState(const planning_models::KinematicState& state)
00410 {
00411   tf::Transform inv = getInverseWorldTransform(state);
00412   for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) {
00413     const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i];
00414     if(body_decomposition_map_.find(ls->getName()) == body_decomposition_map_.end()) {
00415       //ROS_WARN_STREAM("Can't find body decomposition for link state " << ls->getName());
00416       continue;
00417     }
00418     body_decomposition_map_[ls->getName()]->updatePose(inv*ls->getGlobalCollisionBodyTransform());
00419     for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
00420       const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j];
00421       std::string id = makeAttachedObjectId(ls->getName(), att_state->getName());
00422       if(attached_object_map_.find(id) == attached_object_map_.end()) {
00423         ROS_WARN_STREAM("Updating pose but no attached object for id " << id);
00424         continue;
00425       }
00426       if(attached_object_map_[id]->getSize() != att_state->getGlobalCollisionBodyTransforms().size()) {
00427         ROS_WARN_STREAM("For attached object bodies number " << attached_object_map_[id]->getSize() 
00428                         << " not equal to state number " << att_state->getGlobalCollisionBodyTransforms().size() << ". Not updating");
00429         continue;
00430       }
00431       for(unsigned int k = 0; k < att_state->getGlobalCollisionBodyTransforms().size(); k++) {
00432         tf::Transform test = inv*att_state->getGlobalCollisionBodyTransforms()[k];
00433         attached_object_map_[id]->updateBodyPose(k, inv*att_state->getGlobalCollisionBodyTransforms()[k]);
00434       }
00435     }
00436   }
00437 }
00438 
00439 tf::Transform CollisionProximitySpace::getInverseWorldTransform(const planning_models::KinematicState& state) const {
00440   const planning_models::KinematicState::JointState* world_state = state.getJointStateVector()[0];//(monitor_->getKinematicModel()->getRoot()->getName());
00441   if(world_state == NULL) {
00442     ROS_WARN_STREAM("World state " << collision_models_interface_->getKinematicModel()->getRoot()->getName() << " not found");
00443   }
00444   const tf::Transform& world_trans = world_state->getVariableTransform();
00445   tf::Transform ret(world_trans);
00446   return ret.inverse();
00447 }
00448 
00449 void CollisionProximitySpace::syncObjectsWithCollisionSpace(const planning_models::KinematicState& state)
00450 {
00451   tf::Transform inv = getInverseWorldTransform(state);
00452   const collision_space::EnvironmentObjects *eo = collision_models_interface_->getCollisionSpace()->getObjects();
00453   std::vector<std::string> ns = eo->getNamespaces();
00454   for(unsigned int i = 0; i < ns.size(); i++) {
00455     if(ns[i] == COLLISION_MAP_NAME) continue;
00456     const collision_space::EnvironmentObjects::NamespaceObjects &no = eo->getObjects(ns[i]);
00457     BodyDecompositionVector* bdv = new BodyDecompositionVector();
00458     for(unsigned int j = 0; j < no.shape.size(); j++) {
00459       BodyDecomposition* bd = new BodyDecomposition(ns[i]+"_"+makeStringFromUnsignedInt(j), no.shape[j], resolution_);
00460       bd->updatePose(inv*no.shape_pose[j]);
00461       tf::Transform trans = bd->getBody()->getPose();
00462       bdv->addToVector(bd); 
00463     }
00464     static_object_map_[ns[i]] = bdv;
00465   }
00466   
00467   const std::vector<planning_models::KinematicState::LinkState*> link_states = state.getLinkStateVector();
00468   for(unsigned int i = 0; i < link_states.size(); i++) {
00469     const planning_models::KinematicState::LinkState* ls = link_states[i];
00470     for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
00471       BodyDecompositionVector* bdv = new BodyDecompositionVector();
00472       const planning_models::KinematicState::AttachedBodyState* abs = ls->getAttachedBodyStateVector()[j];
00473       std::string id = makeAttachedObjectId(ls->getName(),abs->getName());
00474       for(unsigned int k = 0; k < abs->getAttachedBodyModel()->getShapes().size(); k++) {
00475         BodyDecomposition* bd = new BodyDecomposition(id+makeStringFromUnsignedInt(j), abs->getAttachedBodyModel()->getShapes()[k], resolution_);
00476         bd->updatePose(inv*abs->getGlobalCollisionBodyTransforms()[k]);
00477         bdv->addToVector(bd);
00478       }
00479       attached_object_map_[id] = bdv;
00480       for(unsigned int k = 0; k < abs->getAttachedBodyModel()->getTouchLinks().size(); k++) {
00481         attached_object_collision_links_[id][abs->getAttachedBodyModel()->getTouchLinks()[k]] = false;
00482       }
00483     }
00484   }
00485 }
00486 
00487 void CollisionProximitySpace::setDistanceFieldForGroupQueries(const std::string& group_name,
00488                                                               const planning_models::KinematicState& state)
00489 {
00490   if(enabled_self_collision_links_.find(group_name) == enabled_self_collision_links_.end()) {
00491     ROS_WARN_STREAM("No group named " << group_name << " in planning groups");
00492     return;
00493   }
00494   std::vector<std::string> df_links;
00495   for(std::map<std::string,bool>::iterator it = enabled_self_collision_links_[group_name].begin();
00496       it != enabled_self_collision_links_[group_name].end();
00497       it++) {
00498     if(it->second) {
00499       df_links.push_back(it->first);
00500     }
00501   }
00502   prepareSelfDistanceField(df_links, state);
00503 }
00504 
00505 void CollisionProximitySpace::prepareEnvironmentDistanceField(const planning_models::KinematicState& state)
00506 {
00507   environment_distance_field_->reset();
00508   tf::Transform inv = getInverseWorldTransform(state);
00509   std::vector<tf::Vector3> all_points;
00510   for(std::map<std::string, BodyDecompositionVector*>::iterator it = static_object_map_.begin();
00511       it != static_object_map_.end();
00512       it++) {
00513     for(unsigned int i = 0; i < it->second->getSize(); i++) {
00514       std::vector<tf::Vector3> obj_points = it->second->getBodyDecomposition(i)->getCollisionPoints();
00515       all_points.insert(all_points.end(),obj_points.begin(), obj_points.end());
00516     }
00517   }
00518   for(unsigned int i = 0; i < collision_models_interface_->getCollisionMapPoses().size(); i++) {
00519     all_points.push_back(inv*collision_models_interface_->getCollisionMapPoses()[i].getOrigin());
00520   }
00521   environment_distance_field_->addPointsToField(all_points);
00522   visualizeDistanceField(environment_distance_field_);
00523   //ROS_INFO_STREAM("Adding points took " << (n2-n1).toSec());
00524 }
00525 
00526 void CollisionProximitySpace::prepareSelfDistanceField(const std::vector<std::string>& link_names, 
00527                                                        const planning_models::KinematicState& state)
00528 {
00529   self_distance_field_->reset();
00530   tf::Transform inv = getInverseWorldTransform(state);
00531   std::vector<tf::Vector3> all_points;
00532   for(unsigned int i = 0; i < link_names.size(); i++) {
00533     if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) {
00534       //there is no collision geometry as far as we can tell
00535       continue;
00536     }
00537     //ROS_INFO_STREAM("Adding link " << link_names[i]);
00538     const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second;
00539     all_points.insert(all_points.end(), bd->getCollisionPoints().begin(), bd->getCollisionPoints().end());
00540     const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]);
00541     for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
00542       std::string id = makeAttachedObjectId(ls->getName(),ls->getAttachedBodyStateVector()[j]->getName());
00543       if(attached_object_map_.find(id) == attached_object_map_.end()) {
00544         ROS_WARN_STREAM("Have no attached object body for attached object " << id);
00545         continue;
00546       }
00547       const BodyDecompositionVector* att = attached_object_map_.find(id)->second;
00548       const std::vector<tf::Vector3>& att_points = att->getCollisionPoints();
00549       all_points.insert(all_points.end(), att_points.begin(), att_points.end());
00550     }
00551   }
00552   self_distance_field_->addPointsToField(all_points);
00553 }
00554 
00555 /*
00556 bool CollisionProximitySpace::getEnvironmentProximity(ProximityInfo& prox) const
00557 {
00558   updating_objects_lock_.lock();
00559   prox.proximity = DBL_MAX;
00560   for(unsigned int i = 0; i < link_names.size(); i++) {
00561     if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) {
00562       ROS_WARN_STREAM("No link " << link_names[i] << " for getEnvironmentProximity");
00563       continue;
00564     }
00565     const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second;
00566     unsigned int lc;
00567     tf::Vector3 grad;
00568     double dist = getCollisionSphereProximity(bd->getCollisionSpheres(), lc, grad);
00569     if(dist == 0.0) {
00570       //ROS_WARN_STREAM("Zero distance on link " << link_names[i]);
00571     } else {
00572       //ROS_INFO_STREAM("Link " << link_names[i] << " proximity " << dist);
00573       if(dist < prox.proximity) {
00574         prox.proximity = dist;
00575         prox.link_name = link_names[i];
00576         prox.sphere_index = lc;
00577         prox.closest_point = bd->getCollisionSpheres()[lc].center_;
00578         prox.closest_gradient = grad;
00579       }
00580     }
00581     const planning_models::KinematicModel::LinkModel* lm = monitor_->getKinematicModel()->getLinkModel(link_names[i]);
00582     for(unsigned int j = 0; j < lm->getAttachedBodyModels().size(); j++) {
00583       std::string id = makeAttachedObjectId(lm->getName(),lm->getAttachedBodyModels()[j]->getName());
00584       if(attached_object_map_.find(id) == attached_object_map_.end()) {
00585         ROS_WARN_STREAM("Have no attached object body for attached object " << id);
00586         continue;
00587       }
00588       const BodyDecompositionVector* att = attached_object_map_.find(id)->second;
00589       for(unsigned int k = 0; k < att->getSize(); k++) {
00590         double dist = getCollisionSphereProximity(att->getBodyDecomposition(k)->getCollisionSpheres(), lc, grad);
00591         if(dist < prox.proximity) {
00592           prox.proximity = dist;
00593           prox.link_name = lm->getName();
00594           prox.attached_object_name = id;
00595           prox.att_index = j;
00596           prox.sphere_index = lc;
00597           prox.closest_point = att->getBodyDecomposition(k)->getCollisionSpheres()[lc].center_;
00598           prox.closest_gradient = grad; 
00599         }
00600       }
00601     }
00602   }
00603   updating_objects_lock_.unlock();
00604   return true;
00605 }
00606 
00607 double CollisionProximitySpace::getCollisionSphereProximity(const std::vector<CollisionSphere>& sphere_list, unsigned int& closest, tf::Vector3& gradient) const {
00608   double distance = DBL_MAX;
00609   for(unsigned int i = 0; i < sphere_list.size(); i++) {
00610     tf::Vector3 p = sphere_list[i].center_;
00611     double gx, gy, gz;
00612     double dist = distance_field_->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz);//-sphere_list[i].radius_;
00613     //ROS_INFO_STREAM("Point " << p.x() << " " << p.y() << " " << p.z() << " dist " << dist);
00614     
00615     if(dist < distance) {
00616       distance = dist;
00617       closest = i;
00618       gradient = tf::Vector3(gx,gy,gz);
00619     }
00620   }
00621   return distance;
00622 }
00623 
00624 bool CollisionProximitySpace::getEnvironmentCollision(const std::vector<std::string>& link_names)
00625 {
00626   updating_objects_lock_.lock();
00627   bool any_link_in_coll = false;
00628   for(unsigned int i = 0; i < link_names.size(); i++) {
00629     if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) {
00630       ROS_WARN_STREAM("No link " << link_names[i] << " for getEnvironmentCollision");
00631       continue;
00632     }
00633     const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second;
00634     bool coll = getCollisionSphereCollision(bd->getCollisionSpheres());
00635     if(coll) {
00636       ROS_INFO_STREAM("Link " << link_names[i] << " in collision");
00637       any_link_in_coll = true;
00638     }
00639     const planning_models::KinematicModel::LinkModel* lm = monitor_->getKinematicModel()->getLinkModel(link_names[i]);
00640     for(unsigned int j = 0; j < lm->getAttachedBodyModels().size(); j++) {
00641       std::string id = makeAttachedObjectId(lm->getName(),lm->getAttachedBodyModels()[j]->getName());
00642       if(attached_object_map_.find(id) == attached_object_map_.end()) {
00643         ROS_WARN_STREAM("Have no attached object body for attached object " << id);
00644         continue;
00645       }
00646       const BodyDecompositionVector* att = attached_object_map_[id];
00647       for(unsigned int k = 0; k < att->getSize(); k++) {
00648         coll = getCollisionSphereCollision(att->getBodyDecomposition(k)->getCollisionSpheres());
00649         if(coll) {
00650           ROS_INFO_STREAM("Attached body " << id << " in collision");
00651           any_link_in_coll = true;
00652         }
00653       }
00654     }
00655   }
00656   updating_objects_lock_.unlock();
00657   return any_link_in_coll;
00658 }
00659 */
00660 bool CollisionProximitySpace::getGroupLinkAndAttachedBodyNames(const std::string& group_name,
00661                                                                const planning_models::KinematicState& state,
00662                                                                std::vector<std::string>& link_names,
00663                                                                std::vector<unsigned int>& link_indices,
00664                                                                std::vector<std::string>& attached_body_names,
00665                                                                std::vector<unsigned int>& attached_body_link_indices) const
00666 {
00667   ros::WallTime n1 = ros::WallTime::now();
00668   link_names = collision_models_interface_->getKinematicModel()->getModelGroup(group_name)->getUpdatedLinkModelNames();
00669   if(link_names.size() == 0) {
00670     return false;
00671   }
00672 
00673   //making sure that we have bodies for the links - otherwise they don't have collision geometries
00674   std::vector<std::string>::iterator it = link_names.begin();
00675   while(it != link_names.end()) {
00676     if(body_decomposition_map_.find(*it) == body_decomposition_map_.end()) {
00677       it = link_names.erase(it);
00678     } else {
00679       it++;
00680     }
00681   }
00682 
00683   attached_body_names.clear();
00684   link_indices.clear();
00685   attached_body_link_indices.clear();
00686 
00687   const std::vector<planning_models::KinematicState::LinkState*>& lsv = state.getLinkStateVector();
00688   for(unsigned int i = 0; i < link_names.size(); i++) {
00689     bool found = false;
00690     for(unsigned int j = 0; j < lsv.size(); j++) {
00691       if(lsv[j]->getName() == link_names[i]) {
00692         link_indices.push_back(j);
00693         found = true;
00694         break;
00695       }
00696     }
00697     if(!found) {
00698       ROS_INFO_STREAM("No link state found for link " << link_names[i]);
00699     }
00700     if(collision_models_interface_->getLinkAttachedObjects().find(link_names[i]) != collision_models_interface_->getLinkAttachedObjects().end()) {
00701       unsigned int j = 0;
00702       for(std::map<std::string, bodies::BodyVector*>::const_iterator it = collision_models_interface_->getLinkAttachedObjects().find(link_names[i])->second.begin();
00703           it != collision_models_interface_->getLinkAttachedObjects().find(link_names[i])->second.end();
00704           it++, j++) {
00705         std::string id = makeAttachedObjectId(link_names[i],it->first);
00706         attached_body_names.push_back(id);
00707         attached_body_link_indices.push_back(link_indices[i]);
00708       }
00709     }
00710   }
00711   return true;
00712 }
00713 
00714 bool CollisionProximitySpace::updateSphereLocations(const std::vector<std::string>& link_names,
00715                                                     const std::vector<std::string>& attached_body_names, 
00716                                                     std::vector<GradientInfo>& gradients)
00717 {
00718   if(link_names.size()+attached_body_names.size() != gradients.size()) {
00719     ROS_WARN_STREAM("Updating sphere locations with improperly sized gradients");
00720     return false;
00721   }
00722   for(unsigned int i = 0; i < link_names.size(); i++) {
00723     const std::vector<CollisionSphere>& lcs1 = body_decomposition_map_.find(link_names[i])->second->getCollisionSpheres();
00724     gradients[i].sphere_locations.resize(lcs1.size());
00725     gradients[i].sphere_radii.resize(lcs1.size());
00726     for(unsigned int j = 0; j < lcs1.size(); j++) {
00727       gradients[i].sphere_locations[j] = lcs1[j].center_;
00728       gradients[i].sphere_radii[j] = lcs1[j].radius_;
00729     }
00730   }
00731   unsigned int att_index = link_names.size();
00732   unsigned int att_count = attached_body_names.size();
00733   for(unsigned int i = 0; i < att_count; i++) {
00734     if(attached_object_map_.find(attached_body_names[i]) == attached_object_map_.end()) {
00735       ROS_WARN_STREAM("No attached object " << attached_body_names[i]);
00736       return false;
00737     }
00738     const std::vector<CollisionSphere>& att_vec = attached_object_map_.find(attached_body_names[i])->second->getCollisionSpheres();
00739     gradients[att_index].sphere_locations.resize(att_vec.size());
00740     gradients[att_index].sphere_radii.resize(att_vec.size());
00741     for(unsigned int j = 0; j < att_vec.size(); j++) {
00742       gradients[att_index].sphere_locations[j] = att_vec[j].center_;
00743       gradients[att_index].sphere_radii[j] = att_vec[j].radius_;
00744     }
00745   }
00746   return true;
00747 }
00748 
00749 bool CollisionProximitySpace::setupGradientStructures(const std::vector<std::string>& link_names,
00750                                                       const std::vector<std::string>& attached_body_names, 
00751                                                       std::vector<GradientInfo>& gradients) const
00752 {
00753   if(link_names.size() == 0) {
00754     ROS_WARN_STREAM("No link names in setup gradient structure");
00755     return false;
00756   }
00757   gradients.clear();
00758   unsigned int att_count = attached_body_names.size();
00759 
00760   gradients.resize(link_names.size()+att_count);
00761 
00762   for(unsigned int i = 0; i < link_names.size(); i++) {
00763     const std::vector<CollisionSphere>& lcs1 = body_decomposition_map_.find(link_names[i])->second->getCollisionSpheres();
00764     gradients[i].sphere_locations.resize(lcs1.size());
00765     gradients[i].sphere_radii.resize(lcs1.size());
00766     gradients[i].joint_name = collision_models_interface_->getKinematicModel()->getLinkModel(link_names[i])->getParentJointModel()->getName();
00767     for(unsigned int j = 0; j < lcs1.size(); j++) {
00768       gradients[i].sphere_locations[j] = lcs1[j].center_;
00769       gradients[i].sphere_radii[j] = lcs1[j].radius_;
00770     }
00771     gradients[i].distances.resize(lcs1.size(), DBL_MAX);
00772     gradients[i].gradients.resize(lcs1.size());
00773   }
00774   unsigned int att_index = link_names.size();
00775   for(unsigned int i = 0; i < att_count; i++) {
00776     if(attached_object_map_.find(attached_body_names[i]) == attached_object_map_.end()) {
00777       ROS_WARN_STREAM("No attached object " << attached_body_names[i]);
00778       return false;
00779     }
00780     const std::vector<CollisionSphere>& att_vec = attached_object_map_.find(attached_body_names[i])->second->getCollisionSpheres();
00781     gradients[att_index].sphere_locations.resize(att_vec.size());
00782     gradients[att_index].sphere_radii.resize(att_vec.size());
00783     gradients[att_index].joint_name = collision_models_interface_->getKinematicModel()->getLinkModel(link_names[link_names.size() - 1])->getParentJointModel()->getName();
00784     for(unsigned int j = 0; j < att_vec.size(); j++) {
00785       gradients[att_index].sphere_locations[j] = att_vec[j].center_;
00786       gradients[att_index].sphere_radii[j] = att_vec[j].radius_;
00787     }
00788     gradients[att_index].distances.resize(att_vec.size(), DBL_MAX);
00789     gradients[att_index].gradients.resize(att_vec.size());
00790     att_index++;
00791   }
00792   return true;
00793 }
00794 
00795 bool CollisionProximitySpace::isStateInCollision() const
00796 {
00797   if(isEnvironmentCollision()) return true;
00798   if(isSelfCollision()) return true;
00799   return isIntraGroupCollision();
00800 }
00801 
00802 bool CollisionProximitySpace::getStateCollisions(bool& in_collision, 
00803                                                  std::vector<CollisionType>& collisions) const
00804 {
00805   collisions.clear();
00806   collisions.resize(current_link_names_.size()+current_attached_body_names_.size());
00807   std::vector<bool> env_collisions, intra_collisions, self_collisions;
00808   env_collisions.resize(collisions.size(), false);
00809   intra_collisions = self_collisions = env_collisions;
00810   bool env_collision = getEnvironmentCollisions(env_collisions, false);
00811   bool intra_group_collision = getIntraGroupCollisions(intra_collisions, false);
00812   bool self_collision = getSelfCollisions(intra_collisions, false);
00813   for(unsigned int i = 0; i < current_link_names_.size()+current_attached_body_names_.size(); i++) {
00814     collisions[i].environment = env_collisions[i];
00815     collisions[i].self = self_collisions[i];
00816     collisions[i].intra = intra_collisions[i];
00817   }
00818   if(env_collision || intra_group_collision || self_collision) {
00819     in_collision = true;
00820   } else {
00821     in_collision = false;
00822   }
00823   return in_collision;
00824 }
00825 
00826 bool CollisionProximitySpace::getStateGradients(std::vector<GradientInfo>& gradients,
00827                                                 bool subtract_radii) const
00828 {
00829   gradients = current_gradients_;
00830 
00831   std::vector<GradientInfo> intra_gradients;
00832   std::vector<GradientInfo> self_gradients;
00833   std::vector<GradientInfo> env_gradients;
00834 
00835   bool env_coll = getEnvironmentProximityGradients(env_gradients, subtract_radii);
00836   bool self_coll = getSelfProximityGradients(self_gradients, subtract_radii);
00837   bool intra_coll = getIntraGroupProximityGradients(intra_gradients, subtract_radii);
00838 
00839   for(unsigned int i = 0; i < gradients.size(); i++) {
00840     if(i < current_link_names_.size()) {      
00841       ROS_DEBUG_STREAM("Link " << current_link_names_[i] 
00842                       << " env " << env_gradients[i].closest_distance
00843                       << " self " << self_gradients[i].closest_distance
00844                       << " intra " << intra_gradients[i].closest_distance);
00845     }
00846     {
00847       bool env_at_max = env_gradients[i].closest_distance >= max_environment_distance_;
00848       bool self_at_max = self_gradients[i].closest_distance >= max_self_distance_;
00849       if(env_at_max) {
00850         if(self_at_max || intra_gradients[i].closest_distance < self_gradients[i].closest_distance) {
00851           if(intra_gradients[i].closest_distance == DBL_MAX) {
00852             gradients[i].closest_distance = undefined_distance_;
00853           } else {
00854             gradients[i].closest_distance = intra_gradients[i].closest_distance;          
00855           }
00856           ROS_DEBUG_STREAM("Intra gradient is closest");
00857         } else {
00858           ROS_DEBUG_STREAM("Self gradient is closest");
00859           gradients[i].closest_distance = self_gradients[i].closest_distance;          
00860         }
00861       } else if(self_at_max) {
00862         //don't need to check env_at_max, as the previous condition should take care of it
00863         if(intra_gradients[i].closest_distance < env_gradients[i].closest_distance) {
00864           gradients[i].closest_distance = intra_gradients[i].closest_distance;
00865           ROS_DEBUG_STREAM("Intra gradient is closest");
00866         } else {
00867           gradients[i].closest_distance = env_gradients[i].closest_distance;
00868           ROS_DEBUG_STREAM("Env gradient is closest");
00869         }
00870       } else if(self_gradients[i].closest_distance < env_gradients[i].closest_distance) {
00871         gradients[i].closest_distance = self_gradients[i].closest_distance;
00872           ROS_DEBUG_STREAM("Self gradient is closest");
00873       } else {
00874         gradients[i].closest_distance = env_gradients[i].closest_distance;
00875         ROS_DEBUG_STREAM("Env gradient is closest");
00876       }
00877     }
00878 
00879     if(i < current_link_names_.size() && gradients[i].closest_distance < 0.0) {      
00880       ROS_DEBUG_STREAM("Link " << current_link_names_[i] 
00881                        << " env " << env_gradients[i].closest_distance
00882                        << " self " << self_gradients[i].closest_distance
00883                        << " intra " << intra_gradients[i].closest_distance);
00884     }
00885 
00886 
00887     for(unsigned int j = 0; j < gradients[i].distances.size(); j++) {
00888       bool env_at_max = env_gradients[i].distances[j] >= max_environment_distance_;
00889       bool self_at_max = self_gradients[i].distances[j] >= max_self_distance_;
00890       if(env_at_max) {
00891         if(self_at_max || intra_gradients[i].distances[j] < self_gradients[i].distances[j]) {
00892           if(intra_gradients[i].distances[j] == DBL_MAX) {
00893             gradients[i].distances[j] = undefined_distance_;
00894           } else {
00895             gradients[i].distances[j] = intra_gradients[i].distances[j];
00896           }
00897           gradients[i].gradients[j] = intra_gradients[i].gradients[j]; 
00898        } else {
00899           gradients[i].distances[j] = self_gradients[i].distances[j];
00900           gradients[i].gradients[j] = self_gradients[i].gradients[j];
00901         }
00902       } else if(self_at_max) {
00903         if(intra_gradients[i].distances[j] < env_gradients[i].distances[j]) {
00904           gradients[i].distances[j] = intra_gradients[i].distances[j];
00905           gradients[i].gradients[j] = intra_gradients[i].gradients[j];
00906         } else {
00907           gradients[i].distances[j] = env_gradients[i].distances[j];
00908           gradients[i].gradients[j] = env_gradients[i].gradients[j];
00909         }
00910       } else if(self_gradients[i].distances[j] < env_gradients[i].distances[j]) {
00911         gradients[i].distances[j] = self_gradients[i].distances[j];
00912         gradients[i].gradients[j] = self_gradients[i].gradients[j]; 
00913       } else {
00914         gradients[i].distances[j] = env_gradients[i].distances[j];
00915         gradients[i].gradients[j] = env_gradients[i].gradients[j];
00916       }
00917     }
00918   }
00919   return (env_coll || intra_coll || self_coll);
00920 }
00921 
00922 bool CollisionProximitySpace::isIntraGroupCollision() const
00923 {
00924   std::vector<bool> collisions;
00925   return(getIntraGroupCollisions(collisions, true));
00926 }
00927 
00928 bool CollisionProximitySpace::getIntraGroupCollisions(std::vector<bool>& collisions, bool stop_at_first_collision) const {
00929   bool in_collision = false;
00930   unsigned int num_links = current_link_names_.size();
00931   unsigned int num_attached = current_attached_body_names_.size();
00932   unsigned int tot = num_links+num_attached;
00933   for(unsigned int i = 0; i < tot; i++) {
00934     for(unsigned int j = i; j < tot; j++) {
00935       if(i == j) continue;
00936       if(!current_intra_group_collision_links_[i][j]) continue;
00937       const std::vector<CollisionSphere>* lcs1;
00938       const std::vector<CollisionSphere>* lcs2; 
00939       if(i < num_links) {
00940         lcs1 = &(current_link_body_decompositions_[i]->getCollisionSpheres());
00941       } else {
00942         lcs1 = &(current_attached_body_decompositions_[i-num_links]->getCollisionSpheres());
00943       }
00944       if(j < num_links) {
00945         lcs2 = &(current_link_body_decompositions_[j]->getCollisionSpheres());
00946       } else {
00947         lcs2 = &(current_attached_body_decompositions_[j-num_links]->getCollisionSpheres());
00948       }
00949       for(unsigned int k = 0; k < lcs1->size(); k++) {
00950         for(unsigned int l = 0; l < lcs2->size(); l++) {
00951           double dist = (*lcs1)[k].center_.distance((*lcs2)[l].center_);
00952           dist += -(*lcs1)[k].radius_-(*lcs2)[l].radius_;
00953           if(dist <= tolerance_) {
00954             if(stop_at_first_collision) {
00955               return true;
00956             }
00957             collisions[i] = true;
00958             collisions[j] = true;
00959             in_collision = true;
00960           }
00961         }
00962       }
00963     }
00964   }
00965   return in_collision;
00966 }
00967 
00968 bool CollisionProximitySpace::getIntraGroupProximityGradients(std::vector<GradientInfo>& gradients,
00969                                                               bool subtract_radii) const {
00970   gradients = current_gradients_;
00971   bool in_collision = false;
00972   unsigned int count = 0;
00973   unsigned int num_links = current_link_names_.size();
00974   unsigned int num_attached = current_attached_body_names_.size();
00975   unsigned int tot = num_links+num_attached;
00976   std::vector<std::string> all_names = current_link_names_;
00977   all_names.insert(all_names.end(), current_attached_body_names_.begin(), current_attached_body_names_.end());
00978   for(unsigned int i = 0; i < tot; i++) {
00979     for(unsigned int j = 0; j < tot; j++) {
00980       if(i == j) continue;
00981       if(!current_intra_group_collision_links_[i][j]) {
00982         continue;
00983       }
00984       const std::vector<CollisionSphere>* lcs1;
00985       const std::vector<CollisionSphere>* lcs2; 
00986       if(i < num_links) {
00987         lcs1 = &(current_link_body_decompositions_[i]->getCollisionSpheres());
00988       } else {
00989         lcs1 = &(current_attached_body_decompositions_[i-num_links]->getCollisionSpheres());
00990       }
00991       if(j < num_links) {
00992         lcs2 = &(current_link_body_decompositions_[j]->getCollisionSpheres());
00993       } else {
00994         lcs2 = &(current_attached_body_decompositions_[j-num_links]->getCollisionSpheres());
00995       }
00996       for(unsigned int k = 0; k < lcs1->size(); k++) {
00997         for(unsigned int l = 0; l < lcs2->size(); l++) {
00998           double dist = (*lcs1)[k].center_.distance((*lcs2)[l].center_);
00999           if(subtract_radii) {
01000             dist += -(*lcs1)[k].radius_-(*lcs2)[l].radius_;
01001             if(dist <= tolerance_) {
01002               in_collision = true;
01003             }
01004           }
01005           count++;
01006           if(dist < gradients[i].distances[k]) {
01007             gradients[i].distances[k] = dist;
01008             gradients[i].gradients[k] = (*lcs1)[k].center_-(*lcs2)[l].center_;
01009           }
01010           if(dist < gradients[i].closest_distance) {
01011             gradients[i].closest_distance = dist;
01012           }
01013           if(dist < gradients[j].distances[l]) {
01014             gradients[j].distances[l] = dist;
01015             gradients[j].gradients[l] = (*lcs2)[l].center_-(*lcs1)[k].center_;
01016           }
01017           if(dist < gradients[j].closest_distance) {
01018             gradients[j].closest_distance = dist;
01019           }
01020         }
01021       }
01022     }
01023   }
01024   return in_collision;
01025 }
01026 
01027 bool CollisionProximitySpace::isSelfCollision() const
01028 {
01029   std::vector<bool> collisions;
01030   return(getSelfCollisions(collisions, true));
01031 }
01032 
01033 bool CollisionProximitySpace::getSelfCollisions(std::vector<bool>& collisions,
01034                                                 bool stop_at_first_collision) const
01035 {
01036   bool in_collision = false;
01037   for(unsigned int i = 0; i < current_link_names_.size(); i++) {
01038     const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres();
01039     bool coll = getCollisionSphereCollision(self_distance_field_, body_spheres, tolerance_);
01040     if(coll) {
01041       if(stop_at_first_collision) {
01042         return true;
01043       }
01044       in_collision = true;
01045       collisions[i] = true;
01046     }
01047   }
01048   for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) {
01049     const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres();
01050     bool coll = getCollisionSphereCollision(self_distance_field_, body_spheres, tolerance_);
01051     if(coll) {
01052       if(stop_at_first_collision) {
01053         return true;
01054       }
01055       in_collision = true;
01056       collisions[i+current_link_names_.size()] = true;
01057     }
01058   }
01059   return in_collision;
01060 }
01061 
01062 bool CollisionProximitySpace::getSelfProximityGradients(std::vector<GradientInfo>& gradients,
01063                                                         bool subtract_radii) const {
01064   gradients = current_gradients_;
01065   bool in_collision = false;
01066   for(unsigned int i = 0; i < current_link_names_.size(); i++) {
01067     if(!current_self_excludes_[i]) continue;
01068     const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres();
01069     if(gradients[i].distances.size() != body_spheres.size()) {
01070       ROS_INFO_STREAM("Wrong size for closest distances for link " << current_link_names_[i]);
01071     }
01072     bool coll = getCollisionSphereGradients(self_distance_field_, body_spheres, gradients[i], tolerance_, subtract_radii, max_self_distance_, false);
01073     if(coll) {
01074       in_collision = true;
01075     }
01076   }
01077   for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) {
01078     const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres();
01079     bool coll = getCollisionSphereGradients(self_distance_field_, body_spheres, gradients[i+current_link_names_.size()],
01080                                             tolerance_, subtract_radii, max_self_distance_, false);
01081     if(coll) {
01082       in_collision = true;
01083     }
01084   }
01085   return in_collision;
01086 }
01087 
01088 bool CollisionProximitySpace::isEnvironmentCollision() const
01089 {
01090   std::vector<bool> collisions;
01091   return(getEnvironmentCollisions(collisions, true));
01092 }
01093 
01094 bool CollisionProximitySpace::getEnvironmentCollisions(std::vector<bool>& collisions,
01095                                                        bool stop_at_first_collision) const
01096 {
01097   bool in_collision = false;
01098   for(unsigned int i = 0; i < current_link_names_.size(); i++) {
01099     const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres();
01100     bool coll = getCollisionSphereCollision(environment_distance_field_, body_spheres, tolerance_);
01101     if(coll) {
01102       if(stop_at_first_collision) {
01103         return true;
01104       }
01105       in_collision = true;
01106       collisions[i] = true;
01107     }
01108   }
01109   for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) {
01110     const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres();
01111     bool coll = getCollisionSphereCollision(environment_distance_field_, body_spheres, tolerance_);
01112     if(coll) {
01113       if(stop_at_first_collision) {
01114         return true;
01115       }
01116       in_collision = true;
01117       collisions[i+current_link_names_.size()] = true;
01118     }
01119   }
01120   return in_collision;
01121 }
01122 
01123 bool CollisionProximitySpace::getEnvironmentProximityGradients(std::vector<GradientInfo>& gradients,
01124                                                                bool subtract_radii) const {
01125   gradients = current_gradients_;
01126   bool in_collision = false;
01127   for(unsigned int i = 0; i < current_link_names_.size(); i++) {
01128     const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres();
01129     if(gradients[i].distances.size() != body_spheres.size()) {
01130       ROS_INFO_STREAM("Wrong size for closest distances for link " << current_link_names_[i]);
01131     }
01132     bool coll = getCollisionSphereGradients(environment_distance_field_, body_spheres, gradients[i], tolerance_, subtract_radii, max_environment_distance_, false);
01133     if(coll) {
01134       in_collision = true;
01135     }
01136   }
01137   for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) {
01138     const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres();
01139     bool coll = getCollisionSphereGradients(environment_distance_field_, body_spheres, gradients[i+current_link_names_.size()], tolerance_, subtract_radii, max_environment_distance_, false);
01140     if(coll) {
01141       in_collision = true;
01142     }
01143   }
01144   return in_collision;
01145 }
01146 
01147 void CollisionProximitySpace::getProximityGradientMarkers(const std::vector<std::string>& link_names, 
01148                                                           const std::vector<std::string>& attached_body_names, 
01149                                                           const std::vector<GradientInfo>& gradients,
01150                                                           const std::string& ns, 
01151                                                           visualization_msgs::MarkerArray& arr) const 
01152 {
01153   for(unsigned int i = 0; i < gradients.size(); i++) {
01154     
01155     const std::vector<CollisionSphere>* lcs;
01156     std::string name;
01157     if(i < link_names.size()) {
01158       name = link_names[i];
01159       lcs = &(body_decomposition_map_.find(name)->second->getCollisionSpheres());
01160     } else {
01161       name = attached_body_names[i-link_names.size()];
01162       lcs = &(attached_object_map_.find(name)->second->getCollisionSpheres());
01163     }
01164     for(unsigned int j = 0; j < gradients[i].distances.size(); j++) {
01165       visualization_msgs::Marker arrow_mark;
01166       arrow_mark.header.frame_id = collision_models_interface_->getRobotFrameId();
01167       arrow_mark.header.stamp = ros::Time::now();
01168       if(ns.empty()) {
01169         arrow_mark.ns = "self_coll_gradients";
01170       } else {
01171         arrow_mark.ns = ns;
01172       }
01173       arrow_mark.id = i*1000+j;
01174       double xscale = 0.0;
01175       double yscale = 0.0;
01176       double zscale = 0.0;
01177       if(gradients[i].distances[j] > 0.0) {
01178         if(gradients[i].gradients[j].length() > 0.0) {
01179           xscale = gradients[i].gradients[j].x()/gradients[i].gradients[j].length();
01180           yscale = gradients[i].gradients[j].y()/gradients[i].gradients[j].length();
01181           zscale = gradients[i].gradients[j].z()/gradients[i].gradients[j].length();
01182         } else {
01183           ROS_DEBUG_STREAM("Negative length for " << name << " " << arrow_mark.id << " " << gradients[i].gradients[j].length());
01184         }
01185       } else {
01186         ROS_DEBUG_STREAM("Negative dist for " << name << " " << arrow_mark.id);
01187       }
01188       arrow_mark.points.resize(2);
01189       arrow_mark.points[1].x = (*lcs)[j].center_.x();
01190       arrow_mark.points[1].y = (*lcs)[j].center_.y();
01191       arrow_mark.points[1].z = (*lcs)[j].center_.z();
01192       arrow_mark.points[0] = arrow_mark.points[1];
01193       arrow_mark.points[0].x -= xscale*gradients[i].distances[j];
01194       arrow_mark.points[0].y -= yscale*gradients[i].distances[j];
01195       arrow_mark.points[0].z -= zscale*gradients[i].distances[j];
01196       arrow_mark.scale.x = 0.01;
01197       arrow_mark.scale.y = 0.03;
01198       arrow_mark.color.r = 1.0;
01199       arrow_mark.color.g = 0.2;
01200       arrow_mark.color.b = .5;
01201       arrow_mark.color.a = 1.0;
01202       arr.markers.push_back(arrow_mark);
01203     }
01204   }
01205 }
01206 
01207 CollisionProximitySpace::TrajectorySafety CollisionProximitySpace::isTrajectorySafe(const trajectory_msgs::JointTrajectory& trajectory,
01208                                                                                     const arm_navigation_msgs::Constraints& goal_constraints,
01209                                                                                     const arm_navigation_msgs::Constraints& path_constraints,
01210                                                                                     const std::string& groupName)
01211 {
01212   ROS_DEBUG_NAMED("safety", "Calling isTrajectorySafe");
01213 
01214   collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
01215 
01216   if(!collision_models_interface_->getPlanningSceneState()->hasJointStateGroup(groupName))
01217   {
01218     ROS_ERROR("Group %s does not exist. Cannot evaluate trajectory safety.", groupName.c_str());
01219     return ErrorUnsafe;
01220   }
01221   planning_models::KinematicState::JointStateGroup* stateGroup = collision_models_interface_->getPlanningSceneState()->
01222       getJointStateGroup(groupName);
01223   arm_navigation_msgs::ArmNavigationErrorCodes error_code;
01224   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes;
01225   std::map<std::string, double> stateMap;
01226   collision_models_interface_->getPlanningSceneState()->getKinematicStateValues(stateMap);
01227 
01228   std::vector<arm_navigation_msgs::ContactInformation> contacts;
01229   collision_models_interface_->getAllCollisionsForState(*collision_models_interface_->getPlanningSceneState(),
01230                                                         contacts);
01231                                                         
01232   // if(contacts.size() == 0) {
01233   //   ROS_INFO_STREAM("No contacts for start state");
01234   // } else {
01235   //   for(unsigned int i = 0; i < contacts.size(); i++) {
01236   //     ROS_INFO_STREAM("Contact " << i << " between " << contacts[i].contact_body_1 << " and " << contacts[i].contact_body_2);
01237   //   }
01238   // }
01239   
01240   // If the trajectory is already valid and has no mesh to mesh collisions, we know it's safe.
01241   if(collision_models_interface_->isJointTrajectoryValid(*(collision_models_interface_->getPlanningSceneState()),
01242                                                          trajectory, goal_constraints, path_constraints, error_code, error_codes, false))
01243   {
01244     ROS_INFO_STREAM("Mesh to mesh valid");
01245     return MeshToMeshSafe;
01246   }
01247   // Otherwise we have to do something more clever. We know it may have mesh to mesh collisions, but we might
01248   // want to plan into collision or out of collision. We will still avoid planning through obstacles.
01249   else
01250   {
01251     ROS_DEBUG_STREAM("Mesh to mesh invalid with " << error_code.val);
01252 
01253     std::vector<double> lastDistances;
01254     TrajectoryPointType type = None;
01255     // For each trajectory point, get gradients and assert that the collision cost of start points
01256     // is monotonically decreasing, and that the collision cost of end points is monotonically increasing.
01257     // Midpoints must never have collisions.
01258     //+--------------------------------------------------------------------------------------------------------------+
01259     //|  NONE         | START                                  | MIDDLE                           | END              |
01260     //| Starting Phase| First collision to first non-collision | First non-collision to collision | collision to end |
01261     //| First point.  | Monotonically increasing distance      | No collisions allowed            | Monoton. decrease|
01262     //+--------------------------------------------------------------------------------------------------------------+
01263     for(size_t i = 0; i < trajectory.points.size(); i++)
01264     {
01265       std::vector<GradientInfo> gradients;
01266       std::vector<double> distances;
01267       const trajectory_msgs::JointTrajectoryPoint& trajectoryPoint =  trajectory.points[i];
01268       stateGroup->setKinematicState(trajectoryPoint.positions);
01269       setCurrentGroupState(*(collision_models_interface_->getPlanningSceneState()));
01270 
01271       getStateGradients(gradients, true);
01272       bool in_collision = false;
01273       for(size_t j = 0; j < gradients.size(); j++)
01274       {
01275         GradientInfo& gradient = gradients[j];
01276         in_collision = in_collision || gradient.collision;
01277 
01278         for(size_t k = 0; k < gradient.distances.size(); k++)
01279         {
01280           distances.push_back(gradient.distances[k]);
01281           in_collision = in_collision || gradient.distances[k] < tolerance_;
01282           //if(gradient.distances[k] < tolerance_) {
01283           //  ROS_INFO_STREAM("Point i " << i << " sphere " << k << " distance " << gradient.distances[k] << " less than tolerance " << tolerance_);
01284           //}
01285         }
01286       }
01287 
01288       // If the first point is in collision, we're in the start collision phase
01289       if(type == None && in_collision && i == 0)
01290       {
01291         type = StartCollision;
01292         ROS_DEBUG_NAMED("safety","In starting collision phase at index 0");
01293       }
01294       // If the first point is not in collision, then the first collision we encounter is the end phase.
01295       else if(type == None && in_collision && i != 0)
01296       {
01297         type = EndCollision;
01298         ROS_DEBUG_NAMED("safety","In ending collision phase at index %lu", (long unsigned int)i);
01299       }
01300       // If the first point is not in collision, and the next point is not in collision, then we've entered the
01301       // middle phase.
01302       else if(type == None && !in_collision && i != 0)
01303       {
01304         type = Middle;
01305         ROS_DEBUG_NAMED("safety","In middle collision phase at index %lu",  (long unsigned int)i);
01306       }
01307       // If we've exited collision from the start, then we're in the middle phase.
01308       else if (type == StartCollision && !in_collision)
01309       {
01310         type = Middle;
01311         ROS_DEBUG_NAMED("safety","In middle collision phase at index %lu",  (long unsigned int)i);
01312       }
01313       // If we're in the middle phase, and we encounter a collision, we're in the end phase.
01314       else if(type == Middle && in_collision)
01315       {
01316         type = EndCollision;
01317         ROS_DEBUG_NAMED("safety","In end collision phase at index %lu",  (long unsigned int)i);
01318       }
01319       // If the end phase has collisions, then there should be no point in which it is out of collision.
01320       // We want to avoid passing through obstacles, but not going into contact with obstacles.
01321       else if(type == EndCollision && !in_collision)
01322       {
01323         ROS_DEBUG_NAMED("safety","Got point in the end collision phase that was not in collision : %lu", (long unsigned int) i);
01324         return EndUnsafe;
01325       }
01326 
01327 
01328       for(size_t j = 0; j < distances.size(); j++)
01329       {
01330         double dist = distances[j];
01331         double lastDist = -10000;
01332 
01333         if(lastDistances.size() > j)
01334         {
01335           lastDist = lastDistances[j];
01336         }
01337         else
01338         {
01339           lastDistances.push_back(dist);
01340         }
01341         //ROS_DEBUG_STREAM_NAMED("safety", "Point " << i << " dist " << j << " is " << dist);
01342 
01343         // Only consider spheres in collision.
01344         if(dist > tolerance_)
01345         {
01346           continue;
01347         }
01348 
01349         // Switch occurs in start phase during collision, in end phase during collision, or middle phase
01350         // during non-collision.
01351         switch(type)
01352         {
01353           case StartCollision:
01354             // Assert monotonically increasing.
01355             if(dist < lastDist)
01356             {
01357               ROS_DEBUG_NAMED("safety","Start phase was not monotonically increasing in distance. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j);
01358               return StartUnsafe;
01359             }
01360             break;
01361           case EndCollision:
01362             // Assert monotonically decreasing.
01363             //ROS_INFO_STREAM("Point " << i << " sphere " << j << " lastDist " << lastDist << " dist " << dist);
01364             if(dist > lastDist)
01365             {
01366               ROS_DEBUG_NAMED("safety","End phase was not monotonically decreasing in distance. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j);
01367               return EndUnsafe;
01368             }
01369             break;
01370           case Middle:
01371             // Assert no collisions.
01372             if(in_collision)
01373             {
01374               ROS_DEBUG_NAMED("safety","Middle phase had a collision. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j);
01375               return MiddleUnsafe;
01376             }
01377             break;
01378           case None:
01379             // Nothing.
01380             break;
01381         }
01382 
01383         lastDistances[j] = dist;
01384       }
01385     }
01386     // Trajectory passed all tests.
01387     ROS_DEBUG_NAMED("safety","Trajectory passed all safety tests.");
01388     return InCollisionSafe;
01389   }
01390 
01391 
01392 }
01393 
01395 // Visualization functions
01397   
01398 void CollisionProximitySpace::visualizeDistanceField(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const
01399 {
01400   tf::Transform ident;
01401   ident.setIdentity();
01402   visualization_msgs::Marker marker;
01403   distance_field->getIsoSurfaceMarkers(0.0, 0.0, collision_models_interface_->getWorldFrameId(), ros::Time::now(), ident, marker);
01404   vis_distance_field_marker_publisher_.publish(marker);
01405 }
01406 
01407 void CollisionProximitySpace::visualizeDistanceFieldPlane(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const
01408 {
01409   double length = distance_field->getSize(distance_field::PropagationDistanceField::DIM_X);
01410   double width = distance_field->getSize(distance_field::PropagationDistanceField::DIM_Y);
01411   double height = distance_field->getSize(distance_field::PropagationDistanceField::DIM_Z);
01412   tf::Vector3 origin(distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_X) + length / 2.0,
01413                    distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Y)  + width / 2.0,
01414                    distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Z) + height / 2.0);
01415 
01416   for(double z = distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Z);
01417       z < distance_field->getSize(distance_field::PropagationDistanceField::DIM_Z);
01418       z += distance_field->getResolution(distance_field::PropagationDistanceField::DIM_Z))
01419   {
01420     visualization_msgs::Marker marker;
01421     distance_field->getPlaneMarkers(distance_field::XYPlane, length, width,
01422                                           z, origin,  collision_models_interface_->getWorldFrameId(), ros::Time::now(), marker);
01423     vis_distance_field_marker_publisher_.publish(marker);
01424     ros::Time::sleepUntil(ros::Time::now() + ros::Duration(0.02));
01425     ros::spinOnce();
01426   }
01427 }
01428 /*
01429 void CollisionProximitySpace::visualizeClosestCollisionSpheres(const std::vector<std::string>& link_names) const 
01430 {
01431   planning_models::KinematicState state(monitor_->getKinematicModel());
01432   ProximityInfo prox;
01433   getEnvironmentProximity(current_link_names, prox);
01434   const BodyDecomposition* bsd = body_decomposition_map_.find(prox.link_name)->second;
01435   if(!prox.attached_object_name.empty()) {
01436     bsd = attached_object_map_.find(prox.attached_object_name)->second->getBodyDecomposition(prox.att_index);
01437   }
01438   if(prox.sphere_index < bsd->getCollisionSpheres().size()) {
01439     visualization_msgs::Marker mark;
01440     mark.header.frame_id = monitor_->getRobotFrameId();
01441     mark.header.stamp = ros::Time::now();
01442     mark.type = mark.SPHERE;
01443     mark.ns = "closest_sphere";
01444     mark.id = 0;
01445     mark.scale.x = bsd->getCollisionSpheres()[prox.sphere_index].radius_*2.0;
01446     mark.scale.y = mark.scale.x;
01447     mark.scale.z = mark.scale.x;
01448     mark.color.b = 1.0;
01449     mark.color.a = .5;
01450     mark.pose.position.x = bsd->getCollisionSpheres()[prox.sphere_index].center_.x();
01451     mark.pose.position.y = bsd->getCollisionSpheres()[prox.sphere_index].center_.y();
01452     mark.pose.position.z = bsd->getCollisionSpheres()[prox.sphere_index].center_.z();
01453     mark.pose.orientation.w = 1.0;
01454     vis_marker_publisher_.publish(mark);
01455   }
01456 
01457   std::vector<CollisionSphere> all_collision_spheres;
01458   for(unsigned int i = 0; i < link_names.size(); i++) {
01459     const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]);
01460     if(body_decomposition_map_.find(ls->getName()) == body_decomposition_map_.end()) {
01461       //ROS_WARN_STREAM("Can't find body decomposition for link state " << ls->getName());
01462       continue;
01463     }
01464     const std::vector<CollisionSphere>& body_spheres = body_decomposition_map_.find(ls->getName())->second->getCollisionSpheres();
01465     all_collision_spheres.insert(all_collision_spheres.end(), body_spheres.begin(), body_spheres.end());
01466     for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
01467       const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j];
01468       std::string id = makeAttachedObjectId(ls->getName(), att_state->getName());
01469       if(attached_object_map_.find(id) == attached_object_map_.end()) {
01470         continue;
01471       }
01472       const std::vector<CollisionSphere>& body_spheres2 = attached_object_map_.find(id)->second->getCollisionSpheres();
01473       all_collision_spheres.insert(all_collision_spheres.end(), body_spheres2.begin(), body_spheres2.end());
01474     }
01475   }
01476   std::vector<tf::Vector3> all_collision_gradients;
01477   std::vector<double> distances;
01478   double closest_distance;
01479   getCollisionSphereGradients(all_collision_spheres, closest_distance, distances, all_collision_gradients);
01480   visualization_msgs::MarkerArray arr;
01481   for(unsigned int i = 0; i < all_collision_gradients.size(); i++) {
01482     visualization_msgs::Marker arrow_mark;
01483     arrow_mark.header.frame_id = monitor_->getRobotFrameId();
01484     arrow_mark.header.stamp = ros::Time::now();
01485     arrow_mark.ns = "closest_sphere";
01486     arrow_mark.id = i+1;
01487     double xscale = 0.0;
01488     double yscale = 0.0;
01489     double zscale = 0.0;
01490     if(all_collision_gradients[i].length() > 0.0) {
01491       xscale = all_collision_gradients[i].x()/all_collision_gradients[i].length();
01492       yscale = all_collision_gradients[i].y()/all_collision_gradients[i].length();
01493       zscale = all_collision_gradients[i].z()/all_collision_gradients[i].length();
01494     }
01495     arrow_mark.points.resize(2);
01496     arrow_mark.points[0].x = all_collision_spheres[i].center_.x();
01497     arrow_mark.points[0].y = all_collision_spheres[i].center_.y();
01498     arrow_mark.points[0].z = all_collision_spheres[i].center_.z();
01499     arrow_mark.points[1] = arrow_mark.points[0];
01500     arrow_mark.points[1].x -= xscale*distances[i];
01501     arrow_mark.points[1].y -= yscale*distances[i];
01502     arrow_mark.points[1].z -= zscale*distances[i];
01503     arrow_mark.scale.x = 0.01;
01504     arrow_mark.scale.y = 0.03;
01505     arrow_mark.color.r = 1.0;
01506     arrow_mark.color.g = 0.2;
01507     arrow_mark.color.b = .5;
01508     arrow_mark.color.a = 1.0;
01509     arr.markers.push_back(arrow_mark);
01510   }
01511 
01512   vis_marker_array_publisher_.publish(arr);
01513 }
01514 */
01515 void CollisionProximitySpace::visualizeCollisions(const std::vector<std::string>& link_names, 
01516                                                   const std::vector<std::string>& attached_body_names, 
01517                                                   const std::vector<CollisionType> collisions) const
01518 {
01519   visualization_msgs::MarkerArray arr;
01520 
01521   for(unsigned int i = 0; i < link_names.size(); i++) {
01522     std::string name = link_names[i];
01523     //if(collisions[i] != NONE) {
01524       boost::shared_ptr<const urdf::Link> urdf_link = collision_models_interface_->getParsedDescription()->getLink(name);
01525       if(urdf_link == NULL) {
01526         ROS_INFO_STREAM("No entry in urdf for link " << name);
01527         continue;
01528       }
01529       if(!urdf_link->collision) {
01530         continue;
01531       }
01532       const urdf::Geometry *geom = urdf_link->collision->geometry.get();
01533       if(!geom) {
01534         ROS_DEBUG_STREAM("No collision geometry for link " << name);
01535         continue;
01536       }
01537       const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
01538       if(mesh) {
01539         if (!mesh->filename.empty()) {
01540           visualization_msgs::Marker mark;
01541           mark.header.frame_id = collision_models_interface_->getRobotFrameId();
01542           mark.header.stamp = ros::Time::now();
01543           mark.ns = "proximity_collisions";
01544           mark.id = i;
01545           mark.type = mark.MESH_RESOURCE;
01546           mark.scale.x = 1.05;
01547           mark.scale.y = 1.05;
01548           mark.scale.z = 1.05;
01549           mark.color.a = .5;
01550           if(collisions[i].intra) {
01551             ROS_DEBUG_STREAM(name << " in intra_group");
01552             mark.color.r = 1.0;
01553           } else if(collisions[i].environment) {
01554             ROS_DEBUG_STREAM(name << " in environment");
01555             mark.color.r = 1.0;
01556             mark.color.g = 1.0;
01557           } else if(collisions[i].self) {
01558             ROS_DEBUG_STREAM(name << " in self");
01559             mark.color.b = 1.0;
01560           } else {
01561             mark.color.g = 1.0;
01562           }
01563           //mark.lifetime = ros::Duration(1.0);
01564           mark.frame_locked = true;
01565           const tf::Transform& trans = body_decomposition_map_.find(name)->second->getBody()->getPose();
01566           tf::poseTFToMsg(trans, mark.pose);
01567           mark.mesh_resource = mesh->filename;
01568           arr.markers.push_back(mark);
01569         }
01570       }
01571       //}
01572   }
01573   for(unsigned int i = 0; i < attached_body_names.size(); i++) {
01574     std::string name = attached_body_names[i];
01575     const BodyDecompositionVector* bdv = attached_object_map_.find(attached_body_names[i])->second;
01576     for(unsigned int j = 0; j < bdv->getSize(); j++) {
01577       const BodyDecomposition* bd = bdv->getBodyDecomposition(j);
01578       visualization_msgs::Marker mark;
01579       mark.header.frame_id = collision_models_interface_->getRobotFrameId();
01580       mark.header.stamp = ros::Time::now();
01581       mark.ns = "proximity_collisions";
01582       mark.id = i+link_names.size();      
01583       mark.type = mark.CYLINDER;
01584       bodies::BoundingCylinder cyl;
01585       bd->getBody()->computeBoundingCylinder(cyl);
01586       mark.scale.x = cyl.radius*2.0;
01587       mark.scale.y = cyl.radius*2.0;
01588       mark.scale.z = cyl.length;
01589       mark.color.a = .5;
01590       if(collisions[i+link_names.size()].intra) {
01591         ROS_DEBUG_STREAM(name << " in intra_group");
01592         mark.color.r = 1.0;
01593       } else if(collisions[i+link_names.size()].environment) {
01594         ROS_DEBUG_STREAM(name << " in environment");
01595         mark.color.r = 1.0;
01596         mark.color.g = 1.0;
01597       } else if(collisions[i+link_names.size()].self) {
01598         ROS_DEBUG_STREAM(name << " in self");
01599         mark.color.b = 1.0;
01600       } else {
01601         mark.color.g = 1.0;
01602       }
01603       //mark.lifetime = ros::Duration(1.0);
01604       const tf::Transform& trans = bd->getBody()->getPose();
01605       tf::poseTFToMsg(trans, mark.pose);
01606       arr.markers.push_back(mark);
01607     }
01608   }
01609   vis_marker_array_publisher_.publish(arr);
01610 }
01611 
01612 void CollisionProximitySpace::visualizeObjectVoxels(const std::vector<std::string>& object_names) const
01613 {
01614   visualization_msgs::Marker cube_list;
01615   cube_list.header.frame_id = collision_models_interface_->getRobotFrameId();
01616   cube_list.header.stamp = ros::Time::now();
01617   cube_list.type = cube_list.CUBE_LIST;
01618   cube_list.ns = "body_voxels";
01619   cube_list.id = 1000;
01620   cube_list.scale.x = resolution_;
01621   cube_list.scale.y = resolution_;
01622   cube_list.scale.z = resolution_;
01623   cube_list.color.b = 1.0;
01624   cube_list.color.a = .5;  
01625   for(unsigned int i = 0; i < object_names.size(); i++) {
01626     const std::vector<tf::Vector3>* coll_points;
01627     if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) {
01628       coll_points = &(body_decomposition_map_.find(object_names[i])->second)->getCollisionPoints();
01629     } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) {
01630       coll_points = &(static_object_map_.find(object_names[i])->second)->getCollisionPoints();
01631     } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) {
01632       coll_points = &(attached_object_map_.find(object_names[i])->second)->getCollisionPoints();
01633     } else {
01634       ROS_WARN_STREAM("Don't have object named " << object_names[i]);
01635       continue;
01636     }
01637     for(unsigned int i = 0; i < coll_points->size(); i++) {
01638       geometry_msgs::Point p;
01639       p.x = (*coll_points)[i].x();
01640       p.y = (*coll_points)[i].y();
01641       p.z = (*coll_points)[i].z();
01642       cube_list.points.push_back(p);
01643     }
01644   }
01645   vis_marker_publisher_.publish(cube_list);
01646 }
01647 
01648 void CollisionProximitySpace::visualizeObjectSpheres(const std::vector<std::string>& object_names) const
01649 {
01650   visualization_msgs::MarkerArray arr;
01651   unsigned int count = 0;
01652   for(unsigned int i = 0; i < object_names.size(); i++) {
01653     // visualization_msgs::Marker sphere_list;
01654     // sphere_list.header.frame_id = monitor_->getRobotFrameId();
01655     // sphere_list.header.stamp = ros::Time::now();
01656     // sphere_list.type = sphere_list.SPHERE_LIST;
01657     // sphere_list.ns = "body_spheres";
01658     // sphere_list.id = i;
01659     // sphere_list.color.g = 1.0;
01660     // sphere_list.color.a = .5;  
01661     const std::vector<CollisionSphere>* coll_spheres;
01662     if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) {
01663       coll_spheres = &(body_decomposition_map_.find(object_names[i])->second)->getCollisionSpheres();
01664     } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) {
01665       coll_spheres = &(static_object_map_.find(object_names[i])->second)->getCollisionSpheres();
01666     } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) {
01667       coll_spheres = &(attached_object_map_.find(object_names[i])->second)->getCollisionSpheres();
01668     } else {
01669       ROS_WARN_STREAM("Don't have object named " << object_names[i]);
01670       continue;
01671     }
01672     // sphere_list.scale.x = (*coll_spheres)[0].radius_*2.0;
01673     // sphere_list.scale.y = sphere_list.scale.x;
01674     // sphere_list.scale.z = sphere_list.scale.x;
01675     for(unsigned int i = 0; i < coll_spheres->size(); i++) {
01676       visualization_msgs::Marker sphere;
01677       sphere.header.frame_id = collision_models_interface_->getRobotFrameId();
01678       sphere.header.stamp = ros::Time::now();
01679       sphere.type = sphere.SPHERE;
01680       sphere.ns = "body_spheres";
01681       sphere.id = count++;
01682       sphere.color.g = 1.0;
01683       sphere.color.a = .5;  
01684       sphere.scale.x = (*coll_spheres)[i].radius_*2.0;
01685       sphere.scale.y = sphere.scale.x;
01686       sphere.scale.z = sphere.scale.x;      
01687       geometry_msgs::Point p;
01688       sphere.pose.position.x = (*coll_spheres)[i].center_.x();
01689       sphere.pose.position.y = (*coll_spheres)[i].center_.y();
01690       sphere.pose.position.z = (*coll_spheres)[i].center_.z();
01691       sphere.points.push_back(p);
01692       arr.markers.push_back(sphere);
01693     }
01694   }
01695   vis_marker_array_publisher_.publish(arr);
01696 }
01697 
01698 void CollisionProximitySpace::visualizeBoundingCylinders(const std::vector<std::string>& object_names) const
01699 {
01700   visualization_msgs::MarkerArray arr;
01701   for(unsigned int i = 0; i < object_names.size(); i++) {
01702     visualization_msgs::Marker mark;
01703     mark.header.frame_id = collision_models_interface_->getRobotFrameId();
01704     mark.header.stamp = ros::Time::now();
01705     mark.type = mark.CYLINDER;
01706     mark.ns = "body_cylinders";
01707     mark.id = i;
01708     mark.color.r = 1.0;
01709     mark.color.a = .5;  
01710     const BodyDecomposition* bsd = NULL;
01711     if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) {
01712       bsd = body_decomposition_map_.find(object_names[i])->second;
01713     } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) {
01714       bsd = static_object_map_.find(object_names[i])->second->getBodyDecomposition(0);
01715     } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) {
01716       bsd = attached_object_map_.find(object_names[i])->second->getBodyDecomposition(0);
01717     } else {
01718       ROS_WARN_STREAM("Don't have object named " << object_names[i]);
01719       continue;
01720     }
01721     bodies::BoundingCylinder cyl;
01722     bsd->getBody()->computeBoundingCylinder(cyl);
01723     mark.scale.x = cyl.radius*2.0;
01724     mark.scale.y = cyl.radius*2.0;
01725     mark.scale.z = cyl.length;
01726     mark.pose.position.x = cyl.pose.getOrigin().x();
01727     mark.pose.position.y = cyl.pose.getOrigin().y();
01728     mark.pose.position.z = cyl.pose.getOrigin().z();
01729     mark.pose.orientation.x = cyl.pose.getRotation().x();
01730     mark.pose.orientation.y = cyl.pose.getRotation().y();
01731     mark.pose.orientation.z = cyl.pose.getRotation().z();
01732     mark.pose.orientation.w = cyl.pose.getRotation().w();
01733 
01734     arr.markers.push_back(mark);
01735   }
01736   vis_marker_array_publisher_.publish(arr);
01737 }


collision_proximity
Author(s): Gil Jones
autogenerated on Thu Dec 12 2013 11:08:47