collision_models.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 "planning_environment/models/collision_models.h"
00038 #include "planning_environment/models/model_utils.h"
00039 #include "planning_environment/util/construct_object.h"
00040 #include <collision_space/environmentODE.h>
00041 #include <sstream>
00042 #include <vector>
00043 #include <geometric_shapes/shape_operations.h>
00044 #include <geometric_shapes/body_operations.h>
00045 #include <boost/foreach.hpp>
00046 #include <rosbag/bag.h>
00047 #include <rosbag/view.h>
00048 
00049 inline static std::string stripTFPrefix(const std::string& s) {
00050   
00051   if(s.find_last_of('/') == std::string::npos) {
00052     return s;
00053   }
00054   return s.substr(s.find_last_of('/')+1);
00055 }
00056 
00057 planning_environment::CollisionModels::CollisionModels(const std::string &description) : RobotModels(description)
00058 {
00059   planning_scene_set_ = false;
00060   loadCollisionFromParamServer();
00061 }
00062 
00063 planning_environment::CollisionModels::CollisionModels(boost::shared_ptr<urdf::Model> urdf,
00064                                                        planning_models::KinematicModel* kmodel,
00065                                                        collision_space::EnvironmentModel* ode_collision_model) : RobotModels(urdf, kmodel)
00066 {
00067   ode_collision_model_ = ode_collision_model;
00068 }
00069 
00070 planning_environment::CollisionModels::~CollisionModels(void)
00071 {
00072   deleteAllStaticObjects();
00073   deleteAllAttachedObjects();
00074   shapes::deleteShapeVector(collision_map_shapes_);
00075   delete ode_collision_model_;
00076 }
00077 
00078 void planning_environment::CollisionModels::setupModelFromParamServer(collision_space::EnvironmentModel* model)
00079 {
00080   XmlRpc::XmlRpcValue coll_ops;
00081 
00082   //first we do default collision operations
00083   if(!nh_.hasParam(description_ + "_planning/default_collision_operations")) {
00084     ROS_WARN("No default collision operations specified");
00085   } else {
00086   
00087     nh_.getParam(description_ + "_planning/default_collision_operations", coll_ops);
00088     
00089     if(coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00090       ROS_WARN("default_collision_operations is not an array");
00091       return;
00092     }
00093     
00094     if(coll_ops.size() == 0) {
00095       ROS_WARN("No collision operations in default collision operations");
00096       return;
00097     }
00098     
00099     for(int i = 0; i < coll_ops.size(); i++) {
00100       if(!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation")) {
00101         ROS_WARN("All collision operations must have two objects and an operation");
00102         continue;
00103       }
00104       arm_navigation_msgs::CollisionOperation collision_operation;
00105       collision_operation.object1 = std::string(coll_ops[i]["object1"]);
00106       collision_operation.object2 = std::string(coll_ops[i]["object2"]);
00107       std::string operation = std::string(coll_ops[i]["operation"]);
00108       if(operation == "enable") {
00109         collision_operation.operation =  arm_navigation_msgs::CollisionOperation::ENABLE;
00110       } else if(operation == "disable") {
00111         collision_operation.operation =  arm_navigation_msgs::CollisionOperation::DISABLE;
00112       } else {
00113         ROS_WARN_STREAM("Unrecognized collision operation " << operation << ". Must be enable or disable");
00114         continue;
00115       }
00116       default_collision_operations_.push_back(collision_operation);
00117     }
00118   }
00119 
00120   nh_.param(description_ + "_planning/default_robot_padding", default_padd_, 0.01);
00121   nh_.param(description_ + "_planning/default_robot_scale", default_scale_, 1.0);
00122   nh_.param(description_ + "_planning/default_object_padding", object_padd_, 0.02);
00123   nh_.param(description_ + "_planning/default_attached_padding", attached_padd_, 0.05);
00124 
00125   const std::vector<planning_models::KinematicModel::LinkModel*>& coll_links = kmodel_->getLinkModelsWithCollisionGeometry();
00126   std::map<std::string, double> default_link_padding_map;  
00127   std::vector<std::string> coll_names;
00128   for(unsigned int i = 0; i < coll_links.size(); i++) {
00129     default_link_padding_map[coll_links[i]->getName()] = default_padd_;
00130     coll_names.push_back(coll_links[i]->getName());
00131   }
00132 
00133   ros::NodeHandle priv("~");
00134 
00135   if(priv.hasParam("link_padding")) {
00136     XmlRpc::XmlRpcValue link_padding_xml;
00137     priv.getParam("link_padding", link_padding_xml);
00138     if(link_padding_xml.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00139       ROS_WARN("link_padding is not an array");
00140     } else if(link_padding_xml.size() == 0) {
00141       ROS_WARN("No links specified in link_padding");
00142     } else {
00143       for(int i = 0; i < link_padding_xml.size(); i++) {
00144         if(!link_padding_xml[i].hasMember("link") || !link_padding_xml[i].hasMember("padding")) {
00145           ROS_WARN("Each entry in link padding must specify a link and a padding");
00146           continue;
00147         }
00148         std::string link = std::string(link_padding_xml[i]["link"]);
00149         double padding = link_padding_xml[i]["padding"];
00150         //we don't care if this is a group or what, we're shoving it in
00151         default_link_padding_map[link] = padding;
00152       }
00153     }
00154   }
00155   
00156   //no allowed collisions by default
00157   collision_space::EnvironmentModel::AllowedCollisionMatrix default_collision_matrix(coll_names,false);
00158 
00159   for(std::vector<arm_navigation_msgs::CollisionOperation>::iterator it = default_collision_operations_.begin();
00160       it != default_collision_operations_.end();
00161       it++) {
00162     std::vector<std::string> svec1;
00163     std::vector<std::string> svec2;
00164     if(kmodel_->getModelGroup((*it).object1)) {
00165       svec1 = kmodel_->getModelGroup((*it).object1)->getGroupLinkNames();
00166     } else {
00167       svec1.push_back((*it).object1);
00168     }
00169     if(kmodel_->getModelGroup((*it).object2)) {
00170       svec2 = kmodel_->getModelGroup((*it).object2)->getGroupLinkNames();
00171     } else {
00172       svec2.push_back((*it).object2);
00173     }
00174     default_collision_matrix.changeEntry(svec1, svec2, (*it).operation != arm_navigation_msgs::CollisionOperation::ENABLE);
00175   }
00176 
00177   model->lock();
00178   model->setRobotModel(kmodel_, default_collision_matrix, default_link_padding_map, default_padd_, default_scale_);
00179 
00180   for (unsigned int i = 0 ; i < bounding_planes_.size() / 4 ; ++i)
00181   {
00182     shapes::Plane *plane = new shapes::Plane(bounding_planes_[i * 4], bounding_planes_[i * 4 + 1], bounding_planes_[i * 4 + 2], bounding_planes_[i * 4 + 3]);
00183     model->addObject("bounds", plane);
00184     ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0 for model", bounding_planes_[i * 4], bounding_planes_[i * 4 + 1], bounding_planes_[i * 4 + 2], bounding_planes_[i * 4 + 3]);
00185   }
00186   
00187   model->unlock();    
00188 }
00189 
00190 void planning_environment::CollisionModels::loadCollisionFromParamServer()
00191 {
00192   // a list of static planes bounding the environment
00193   bounding_planes_.clear();
00194     
00195   std::string planes;
00196   nh_.param<std::string>("bounding_planes", planes, std::string());
00197     
00198   std::stringstream ss(planes);
00199   if (!planes.empty())
00200     while (ss.good() && !ss.eof())
00201     {
00202       double value;
00203       ss >> value;
00204       bounding_planes_.push_back(value);
00205     }
00206   if (bounding_planes_.size() % 4 != 0)
00207   {
00208     ROS_WARN("~bounding_planes must be a list of 4-tuples (a b c d) that define planes ax+by+cz+d=0");
00209     bounding_planes_.resize(bounding_planes_.size() - (bounding_planes_.size() % 4));
00210   }
00211 
00212   if (loadedModels())
00213   {
00214     ode_collision_model_ = new collision_space::EnvironmentModelODE();
00215     setupModelFromParamServer(ode_collision_model_);
00216         
00217     //  bullet_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelBullet());
00218     //  setupModel(bullet_collision_model_, links);
00219   } else {
00220     ROS_WARN("Models not loaded");
00221   }
00222 }
00223 
00227 
00228 planning_models::KinematicState* 
00229 planning_environment::CollisionModels::setPlanningScene(const arm_navigation_msgs::PlanningScene& planning_scene) {
00230 
00231   if(planning_scene_set_) {
00232     ROS_WARN("Must revert before setting planning scene again");
00233     return NULL;
00234   }
00235   //anything we've already got should go back to default
00236   deleteAllStaticObjects();
00237   deleteAllAttachedObjects();
00238   revertAllowedCollisionToDefault();
00239   revertCollisionSpacePaddingToDefault();
00240   clearAllowedContacts();
00241 
00242   scene_transform_map_.clear();
00243 
00244   for(unsigned int i = 0; i < planning_scene.fixed_frame_transforms.size(); i++) {
00245     if(planning_scene.fixed_frame_transforms[i].header.frame_id != getWorldFrameId()) {
00246       ROS_WARN_STREAM("Fixed transform for " << planning_scene.fixed_frame_transforms[i].child_frame_id 
00247                       << " has non-fixed header frame "  << planning_scene.fixed_frame_transforms[i].header.frame_id);
00248       return NULL;
00249     }
00250     scene_transform_map_[planning_scene.fixed_frame_transforms[i].child_frame_id] = planning_scene.fixed_frame_transforms[i];
00251   }
00252 
00253   planning_models::KinematicState* state = new planning_models::KinematicState(kmodel_);
00254   bool complete = setRobotStateAndComputeTransforms(planning_scene.robot_state, *state);
00255   if(!complete) {
00256     ROS_WARN_STREAM("Incomplete robot state in setPlanningScene");
00257     delete state;
00258     return NULL;
00259   }
00260   std::vector<arm_navigation_msgs::CollisionObject> conv_objects;
00261   std::vector<arm_navigation_msgs::AttachedCollisionObject> conv_att_objects;
00262 
00263   //need to do conversions first so we can delet the planning state
00264   for(unsigned int i = 0; i < planning_scene.collision_objects.size(); i++) {
00265     if(planning_scene.collision_objects[i].operation.operation != arm_navigation_msgs::CollisionObjectOperation::ADD) {
00266       ROS_WARN_STREAM("Planning scene shouldn't have collision operations other than add");
00267       delete state;
00268       return NULL;
00269     }
00270     conv_objects.push_back(planning_scene.collision_objects[i]);
00271     convertCollisionObjectToNewWorldFrame(*state, conv_objects.back());
00272   }
00273   for(unsigned int i = 0; i < planning_scene.attached_collision_objects.size(); i++) {
00274     if(planning_scene.attached_collision_objects[i].object.operation.operation != arm_navigation_msgs::CollisionObjectOperation::ADD) {
00275       ROS_WARN_STREAM("Planning scene shouldn't have collision operations other than add");
00276       delete state;
00277       return NULL;
00278     }
00279     conv_att_objects.push_back(planning_scene.attached_collision_objects[i]);
00280     convertAttachedCollisionObjectToNewWorldFrame(*state, conv_att_objects.back());
00281   }
00282 
00283   //now we delete temp_state to release the lock
00284   delete state;
00285   
00286   for(unsigned int i = 0; i < conv_objects.size(); i++) {
00287     addStaticObject(conv_objects[i]);
00288   }
00289   for(unsigned int i = 0; i < conv_att_objects.size(); i++) {
00290     addAttachedObject(conv_att_objects[i]);
00291   }
00292 
00293   //now we create again after adding the attached objects
00294   state = new planning_models::KinematicState(kmodel_);
00295   setRobotStateAndComputeTransforms(planning_scene.robot_state, *state);  
00296 
00297   //this updates the attached bodies before we mask the collision map
00298   updateAttachedBodyPoses(*state);
00299 
00300   //TODO - allowed contacts
00301   //have to call this first, because it reverts the allowed collision matrix
00302   setCollisionMap(planning_scene.collision_map, true);
00303 
00304   if(planning_scene.link_padding.size() > 0) {
00305     applyLinkPaddingToCollisionSpace(planning_scene.link_padding);
00306   }
00307 
00308   std::vector<arm_navigation_msgs::AllowedContactSpecification> acmv = planning_scene.allowed_contacts;
00309   for(unsigned int i = 0; i < planning_scene.allowed_contacts.size(); i++) {
00310     
00311     if(!convertPoseGivenWorldTransform(*state, 
00312                                        getWorldFrameId(),
00313                                        planning_scene.allowed_contacts[i].pose_stamped.header,
00314                                        planning_scene.allowed_contacts[i].pose_stamped.pose,
00315                                        acmv[i].pose_stamped)) {
00316       ROS_WARN_STREAM("Can't transform allowed contact from frame " << planning_scene.allowed_contacts[i].pose_stamped.header.frame_id
00317                       << " into " << getWorldFrameId());
00318     }
00319   }
00320 
00321   if(!planning_scene.allowed_contacts.empty()) {
00322     std::vector<collision_space::EnvironmentModel::AllowedContact> acv;
00323     convertAllowedContactSpecificationMsgToAllowedContactVector(acmv, 
00324                                                               acv);
00325     ode_collision_model_->lock();    
00326     ode_collision_model_->setAllowedContacts(acv);
00327     ode_collision_model_->unlock();    
00328   }
00329 
00330   if(!planning_scene.allowed_collision_matrix.link_names.empty()) {
00331     ode_collision_model_->lock();
00332     ode_collision_model_->setAlteredCollisionMatrix(convertFromACMMsgToACM(planning_scene.allowed_collision_matrix));
00333     ode_collision_model_->unlock();
00334   } 
00335   planning_scene_set_ = true;
00336   return state;
00337 }
00338 
00339 void planning_environment::CollisionModels::revertPlanningScene(planning_models::KinematicState* ks) {
00340   bodiesLock();
00341   planning_scene_set_ = false;
00342   delete ks;
00343   deleteAllStaticObjects();
00344   deleteAllAttachedObjects();
00345   revertAllowedCollisionToDefault();
00346   revertCollisionSpacePaddingToDefault();
00347   clearAllowedContacts();
00348   bodiesUnlock();
00349 }
00350 
00354 
00355 bool planning_environment::CollisionModels::convertPoseGivenWorldTransform(const planning_models::KinematicState& state,
00356                                                                            const std::string& des_frame_id,
00357                                                                            const std_msgs::Header& header,
00358                                                                            const geometry_msgs::Pose& pose,
00359                                                                            geometry_msgs::PoseStamped& ret_pose) const
00360 {
00361   ret_pose.header = header;
00362   ret_pose.pose = pose;
00363   
00364   std::string r_header_frame_id = stripTFPrefix(header.frame_id);
00365   std::string r_des_frame_id = stripTFPrefix(des_frame_id);
00366 
00367   bool header_is_fixed_frame = (r_header_frame_id == getWorldFrameId());
00368   bool des_is_fixed_frame = (r_des_frame_id == getWorldFrameId());
00369 
00370   //Scenario 1(fixed->fixed): if pose is in the world frame and
00371   //desired is in the world frame, just return
00372   if(header_is_fixed_frame && des_is_fixed_frame) {
00373     return true;
00374   }
00375   const planning_models::KinematicState::LinkState* header_link_state = state.getLinkState(r_header_frame_id);
00376   const planning_models::KinematicState::LinkState* des_link_state = state.getLinkState(r_des_frame_id);
00377   
00378   bool header_is_robot_frame = (header_link_state != NULL);
00379   bool des_is_robot_frame = (des_link_state != NULL);
00380 
00381   bool header_is_other_frame = !header_is_fixed_frame && !header_is_robot_frame;
00382   bool des_is_other_frame = !des_is_fixed_frame && !des_is_robot_frame;
00383 
00384   //Scenario 2(*-> other): We can't deal with desired being in a
00385   //non-fixed frame or relative to the robot.  TODO - decide if this is useful
00386   if(des_is_other_frame) {
00387     ROS_WARN_STREAM("Shouldn't be transforming into non-fixed non-robot frame " << r_des_frame_id);
00388     return false;
00389   }
00390 
00391   //Scenario 3 (other->fixed) && 4 (other->robot): we first need to
00392   //transform into the fixed frame
00393   if(header_is_other_frame) {
00394     if(scene_transform_map_.find(r_header_frame_id) == scene_transform_map_.end()) {
00395       ROS_WARN_STREAM("Trying to transform from other frame " << r_header_frame_id << " to " << r_des_frame_id << " but planning scene doesn't have other frame");
00396       return false;
00397     }
00398     geometry_msgs::TransformStamped trans = scene_transform_map_.find(r_header_frame_id)->second;
00399     
00400     tf::Transform tf_trans;
00401     tf::transformMsgToTF(trans.transform, tf_trans);
00402     tf::Transform tf_pose;
00403     tf::poseMsgToTF(ret_pose.pose, tf_pose);
00404     
00405     tf::Transform fpose = tf_trans*tf_pose;
00406 
00407     //assumes that we've already checked that this matched the world frame
00408     ret_pose.header.frame_id = getWorldFrameId();
00409     tf::poseTFToMsg(fpose, ret_pose.pose);
00410 
00411     if(des_is_fixed_frame) {
00412       return true;
00413     }
00414   }
00415   
00416   //getting tf version of pose
00417   tf::Transform bt_pose;
00418   tf::poseMsgToTF(ret_pose.pose,bt_pose);
00419 
00420   //Scenarios 4(other->robot)/5(fixed->robot): We either started out
00421   //with a header frame in the fixed frame or converted from a
00422   //non-robot frame into the fixed frame, and now we need to transform
00423   //into the desired robot frame given the new world transform
00424   if(ret_pose.header.frame_id == getWorldFrameId() && des_is_robot_frame) {
00425     tf::Transform trans_bt_pose = des_link_state->getGlobalLinkTransform().inverse()*bt_pose;
00426     tf::poseTFToMsg(trans_bt_pose,ret_pose.pose);
00427     ret_pose.header.frame_id = des_link_state->getName();
00428   } else if(header_is_robot_frame && des_is_fixed_frame) {
00429     //Scenario 6(robot->fixed): Just need to look up robot transform and pre-multiply
00430     tf::Transform trans_bt_pose = header_link_state->getGlobalLinkTransform()*bt_pose;
00431     tf::poseTFToMsg(trans_bt_pose,ret_pose.pose);
00432     ret_pose.header.frame_id = getWorldFrameId();
00433   } else if(header_is_robot_frame && des_is_robot_frame) {
00434     //Scenario 7(robot->robot): Completely tf independent
00435     tf::Transform trans_bt_pose = des_link_state->getGlobalLinkTransform().inverse()*(header_link_state->getGlobalLinkTransform()*bt_pose);
00436     tf::poseTFToMsg(trans_bt_pose,ret_pose.pose);
00437     ret_pose.header.frame_id = des_link_state->getName();
00438   } else {
00439     ROS_WARN("Really shouldn't have gotten here");
00440     return false;
00441   }
00442   return true;
00443 }
00444 
00445 bool planning_environment::CollisionModels::convertAttachedCollisionObjectToNewWorldFrame(const planning_models::KinematicState& state,
00446                                                                                           arm_navigation_msgs::AttachedCollisionObject& att_obj) const
00447 {
00448   for(unsigned int i = 0; i < att_obj.object.poses.size(); i++) {
00449     geometry_msgs::PoseStamped ret_pose;    
00450     if(!convertPoseGivenWorldTransform(state,
00451                                        att_obj.link_name,
00452                                        att_obj.object.header,
00453                                        att_obj.object.poses[i],
00454                                        ret_pose)) {
00455       return false;
00456     }
00457     if(i == 0) {
00458       att_obj.object.header = ret_pose.header;
00459     }
00460     att_obj.object.poses[i] = ret_pose.pose;
00461   }
00462   return true;
00463 }
00464 
00465 bool planning_environment::CollisionModels::convertCollisionObjectToNewWorldFrame(const planning_models::KinematicState& state,
00466                                                                                   arm_navigation_msgs::CollisionObject& obj) const
00467 {
00468   for(unsigned int i = 0; i < obj.poses.size(); i++) {
00469     geometry_msgs::PoseStamped ret_pose;
00470     if(!convertPoseGivenWorldTransform(state,
00471                                        getWorldFrameId(),
00472                                        obj.header,
00473                                        obj.poses[i],
00474                                        ret_pose)) {
00475       return false;
00476     }
00477     if(i == 0) {
00478       obj.header = ret_pose.header;
00479     }
00480     obj.poses[i] = ret_pose.pose;
00481   }
00482   return true;
00483 }
00484 
00485 bool planning_environment::CollisionModels::convertConstraintsGivenNewWorldTransform(const planning_models::KinematicState& state,
00486                                                                                      arm_navigation_msgs::Constraints& constraints,
00487                                                                                      const std::string& opt_frame) const {
00488   std::string trans_frame;
00489   if(!opt_frame.empty()) {
00490     trans_frame = opt_frame;
00491   } else {
00492     trans_frame = getWorldFrameId();
00493   }
00494   for(unsigned int i = 0; i < constraints.position_constraints.size(); i++) {
00495     geometry_msgs::PointStamped ps;
00496     if(!convertPointGivenWorldTransform(state,
00497                                         trans_frame,
00498                                         constraints.position_constraints[i].header,
00499                                         constraints.position_constraints[i].position,
00500                                         ps)) {
00501       return false;
00502     }
00503     constraints.position_constraints[i].header = ps.header;
00504     constraints.position_constraints[i].position = ps.point;
00505     
00506     geometry_msgs::QuaternionStamped qs;
00507     if(!convertQuaternionGivenWorldTransform(state,
00508                                              trans_frame,                                     
00509                                              constraints.position_constraints[i].header,
00510                                              constraints.position_constraints[i].constraint_region_orientation,
00511                                              qs)) {
00512       return false;
00513     }
00514     constraints.position_constraints[i].constraint_region_orientation = qs.quaternion; 
00515   }
00516   
00517   for(unsigned int i = 0; i < constraints.orientation_constraints.size(); i++) {
00518     geometry_msgs::QuaternionStamped qs;
00519     if(!convertQuaternionGivenWorldTransform(state,
00520                                              trans_frame,
00521                                              constraints.orientation_constraints[i].header,
00522                                              constraints.orientation_constraints[i].orientation,
00523                                              qs)) {
00524       return false;
00525     }
00526     constraints.orientation_constraints[i].header = qs.header;
00527     constraints.orientation_constraints[i].orientation = qs.quaternion;             
00528   }
00529   
00530   for(unsigned int i = 0; i < constraints.visibility_constraints.size(); i++) {
00531     if(!convertPointGivenWorldTransform(state,
00532                                         trans_frame,
00533                                         constraints.visibility_constraints[i].target.header,
00534                                         constraints.visibility_constraints[i].target.point,
00535                                         constraints.visibility_constraints[i].target)) {
00536       return false;
00537     }
00538   }  
00539   return true;
00540 }
00541 
00542 bool planning_environment::CollisionModels::convertPointGivenWorldTransform(const planning_models::KinematicState& state,
00543                                                                             const std::string& des_frame_id,
00544                                                                             const std_msgs::Header& header,
00545                                                                             const geometry_msgs::Point& point,
00546                                                                             geometry_msgs::PointStamped& ret_point) const
00547 {
00548   geometry_msgs::Pose arg_pose;
00549   arg_pose.position = point;
00550   arg_pose.orientation.w = 1.0;
00551   geometry_msgs::PoseStamped ret_pose;
00552   if(!convertPoseGivenWorldTransform(state, 
00553                                      des_frame_id,
00554                                      header,
00555                                      arg_pose,
00556                                      ret_pose)) {
00557     return false;
00558   }
00559   ret_point.header = ret_pose.header;
00560   ret_point.point = ret_pose.pose.position;
00561   return true;
00562 }
00563 
00564 bool planning_environment::CollisionModels::convertQuaternionGivenWorldTransform(const planning_models::KinematicState& state,
00565                                                                                  const std::string& des_frame_id,
00566                                                                                  const std_msgs::Header& header,
00567                                                                                  const geometry_msgs::Quaternion& quat,
00568                                                                                  geometry_msgs::QuaternionStamped& ret_quat) const
00569 {
00570   geometry_msgs::Pose arg_pose;
00571   arg_pose.orientation = quat;
00572   geometry_msgs::PoseStamped ret_pose;
00573   if(!convertPoseGivenWorldTransform(state, 
00574                                      des_frame_id,
00575                                      header,
00576                                      arg_pose,
00577                                      ret_pose)) {
00578     return false;
00579   }
00580   ret_quat.header = ret_pose.header;
00581   ret_quat.quaternion = ret_pose.pose.orientation;
00582   return true;
00583 }
00584 
00585 bool planning_environment::CollisionModels::updateAttachedBodyPosesForLink(const planning_models::KinematicState& state,
00586                                                                            const std::string& link_name)
00587 {
00588   bodiesLock();
00589   if(link_attached_objects_.find(link_name) == link_attached_objects_.end()) {
00590     bodiesUnlock();
00591     return false;
00592   }
00593   const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_name);
00594   for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) {
00595     const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j];
00596     std::map<std::string, bodies::BodyVector*>::iterator bvit = link_attached_objects_[link_name].find(att_state->getName());
00597     if(bvit == link_attached_objects_[link_name].end()) {
00598       ROS_WARN_STREAM("State out of sync with attached body vector for attached body " << att_state->getName());
00599       bodiesUnlock();
00600       return false;
00601     }
00602     if(bvit->second->getSize() != att_state->getGlobalCollisionBodyTransforms().size()) {
00603       ROS_WARN_STREAM("State out of sync with attached body vector for attached body " << att_state->getName());
00604       bodiesUnlock();
00605       return false;
00606     }
00607     for(unsigned int k = 0; k < att_state->getGlobalCollisionBodyTransforms().size(); k++) {
00608       bvit->second->setPose(k, att_state->getGlobalCollisionBodyTransforms()[k]);
00609     }
00610   }
00611   bodiesUnlock();
00612   return true;
00613 }
00614 
00615 bool planning_environment::CollisionModels::updateAttachedBodyPoses(const planning_models::KinematicState& state)
00616 {
00617   for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::iterator it = link_attached_objects_.begin();
00618       it != link_attached_objects_.end();
00619       it++) {
00620     if(!updateAttachedBodyPosesForLink(state, it->first)) {
00621       return false;
00622     }
00623   }
00624   return true;
00625 }
00626 
00627 bool planning_environment::CollisionModels::addStaticObject(const arm_navigation_msgs::CollisionObject& obj)
00628 {
00629   std::vector<shapes::Shape*> shapes;
00630   std::vector<tf::Transform> poses;
00631   for(unsigned int i = 0; i < obj.shapes.size(); i++) {
00632     shapes::Shape *shape = constructObject(obj.shapes[i]);
00633     if(!shape) {
00634       ROS_WARN_STREAM("Something wrong with shape");
00635       return false;
00636     }
00637     shapes.push_back(shape);
00638     tf::Transform pose;
00639     tf::poseMsgToTF(obj.poses[i], pose);
00640     poses.push_back(pose);
00641   }
00642   double padding = object_padd_;
00643   if(obj.padding < 0.0) {
00644     padding = 0.0;
00645   } else if(obj.padding > 0.0){
00646     padding = obj.padding;
00647   }
00648   addStaticObject(obj.id,
00649                   shapes, 
00650                   poses,
00651                   padding);
00652   return true;
00653 }
00654 
00655 //note - ownership of shape passes in
00656 void planning_environment::CollisionModels::addStaticObject(const std::string& name,
00657                                                             std::vector<shapes::Shape*>& shapes,
00658                                                             const std::vector<tf::Transform>& poses,
00659                                                             double padding)
00660 {
00661   if(ode_collision_model_->hasObject(name)) {
00662     deleteStaticObject(name);
00663   }
00664   bodiesLock();
00665   static_object_map_[name] = new bodies::BodyVector(shapes, poses, padding);
00666   ode_collision_model_->lock();
00667   ode_collision_model_->addObjects(name, shapes, poses);
00668   ode_collision_model_->unlock();
00669   bodiesUnlock();
00670 }
00671 
00672 void planning_environment::CollisionModels::deleteStaticObject(const std::string& name)
00673 {
00674   bodiesLock();
00675   if(!ode_collision_model_->hasObject(name)) {
00676     return;
00677   }
00678   delete static_object_map_.find(name)->second;
00679   static_object_map_.erase(name);
00680   ode_collision_model_->lock();
00681   ode_collision_model_->clearObjects(name);
00682   ode_collision_model_->unlock();
00683   bodiesUnlock();
00684 }
00685 
00686 void planning_environment::CollisionModels::deleteAllStaticObjects() {
00687   bodiesLock();
00688   for(std::map<std::string, bodies::BodyVector*>::iterator it = static_object_map_.begin();
00689       it != static_object_map_.end();
00690       it++) {
00691     delete it->second;
00692   }
00693   static_object_map_.clear();
00694   ode_collision_model_->lock();
00695   ode_collision_model_->clearObjects();
00696   ode_collision_model_->unlock();
00697   bodiesUnlock();
00698 }
00699 
00700 void planning_environment::CollisionModels::setCollisionMap(const arm_navigation_msgs::CollisionMap& map, 
00701                                                             bool mask_before_insertion) {
00702   std::vector<shapes::Shape*> shapes(map.boxes.size());
00703   std::vector<tf::Transform> poses;
00704   for(unsigned int i = 0; i < map.boxes.size(); i++) {
00705     shapes[i] = new shapes::Box(map.boxes[i].extents.x, map.boxes[i].extents.y, map.boxes[i].extents.z);
00706     tf::Transform pose;
00707     pose.setOrigin(tf::Vector3(map.boxes[i].center.x, map.boxes[i].center.y, map.boxes[i].center.z));
00708     pose.setRotation(tf::Quaternion(tf::Vector3(map.boxes[i].axis.x, map.boxes[i].axis.y, map.boxes[i].axis.z), map.boxes[i].angle));
00709     poses.push_back(pose);
00710   }
00711   setCollisionMap(shapes, poses, mask_before_insertion);
00712 }
00713 
00714 void planning_environment::CollisionModels::setCollisionMap(std::vector<shapes::Shape*>& shapes,
00715                                                             const std::vector<tf::Transform>& poses,
00716                                                             bool mask_before_insertion)
00717 {
00718   bodiesLock();
00719   shapes::deleteShapeVector(collision_map_shapes_);
00720   collision_map_shapes_ = shapes::cloneShapeVector(shapes);
00721   collision_map_poses_ = poses;
00722   std::vector<tf::Transform> masked_poses = poses;
00723   if(mask_before_insertion) {
00724     maskAndDeleteShapeVector(shapes,masked_poses); 
00725   } 
00726   ode_collision_model_->lock();
00727   ode_collision_model_->clearObjects(COLLISION_MAP_NAME);
00728   if(shapes.size() > 0) {
00729     ode_collision_model_->addObjects(COLLISION_MAP_NAME, shapes, masked_poses);
00730   } else {
00731     ROS_DEBUG_STREAM("Not setting any collision map objects");
00732   }
00733   ode_collision_model_->unlock();
00734   bodiesUnlock();
00735 }
00736 
00737 void planning_environment::CollisionModels::remaskCollisionMap() {
00738   std::vector<shapes::Shape*> shapes = shapes::cloneShapeVector(collision_map_shapes_);
00739   std::vector<tf::Transform> masked_poses = collision_map_poses_;
00740   maskAndDeleteShapeVector(shapes,masked_poses); 
00741   ode_collision_model_->lock();
00742   ode_collision_model_->clearObjects(COLLISION_MAP_NAME);
00743   ode_collision_model_->addObjects(COLLISION_MAP_NAME, shapes, masked_poses);
00744   ode_collision_model_->unlock();
00745 }
00746 
00747 void planning_environment::CollisionModels::maskAndDeleteShapeVector(std::vector<shapes::Shape*>& shapes,
00748                                                                      std::vector<tf::Transform>& poses)
00749 {
00750   bodiesLock();
00751   std::vector<bool> mask;
00752   std::vector<bodies::BodyVector*> object_vector;
00753   //masking out static objects
00754   for(std::map<std::string, bodies::BodyVector*>::iterator it = static_object_map_.begin();
00755       it != static_object_map_.end();
00756       it++) {
00757     object_vector.push_back(it->second);
00758   }
00759   //also masking out attached objects
00760   for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::iterator it = link_attached_objects_.begin();
00761       it != link_attached_objects_.end();
00762       it++) {
00763     for(std::map<std::string, bodies::BodyVector*>::iterator it2 = it->second.begin();
00764         it2 != it->second.end();
00765         it2++) {
00766       object_vector.push_back(it2->second);
00767     }    
00768   }
00769   bodies::maskPosesInsideBodyVectors(poses, object_vector, mask, true);
00770   std::vector<tf::Transform> ret_poses;
00771   std::vector<shapes::Shape*> ret_shapes;
00772   unsigned int num_masked = 0;
00773   for(unsigned int i = 0; i < mask.size(); i++) {
00774     if(mask[i]) {
00775       ret_shapes.push_back(shapes[i]);
00776       ret_poses.push_back(poses[i]);
00777     } else {
00778       num_masked++;
00779       delete shapes[i];
00780     }
00781   }
00782   shapes = ret_shapes;
00783   poses = ret_poses;
00784   bodiesUnlock();
00785 }
00786 
00787 bool planning_environment::CollisionModels::addAttachedObject(const arm_navigation_msgs::AttachedCollisionObject& att)
00788 {
00789   const arm_navigation_msgs::CollisionObject& obj = att.object;
00790   std::vector<shapes::Shape*> shapes;
00791   std::vector<tf::Transform> poses;
00792   for(unsigned int i = 0; i < obj.shapes.size(); i++) {
00793     shapes::Shape *shape = constructObject(obj.shapes[i]);
00794     if(!shape) {
00795       ROS_WARN_STREAM("Something wrong with shape");
00796       return false;
00797     }
00798     shapes.push_back(shape);
00799     tf::Transform pose;
00800     tf::poseMsgToTF(obj.poses[i], pose);
00801     poses.push_back(pose);
00802   }
00803   double padding = attached_padd_;
00804   if(obj.padding < 0.0) {
00805     padding = 0.0;
00806   } else if(obj.padding > 0.0){
00807     padding = obj.padding;
00808   }
00809   if(!addAttachedObject(obj.id,
00810                         att.link_name,
00811                         shapes,
00812                         poses,
00813                         att.touch_links,
00814                         padding)) {
00815     ROS_INFO_STREAM("Problem attaching " << obj.id);
00816     shapes::deleteShapeVector(shapes);
00817   }
00818   return true;
00819 }
00820 
00821 
00822 bool planning_environment::CollisionModels::addAttachedObject(const std::string& object_name,
00823                                                               const std::string& link_name,
00824                                                               std::vector<shapes::Shape*>& shapes,
00825                                                               const std::vector<tf::Transform>& poses,
00826                                                               const std::vector<std::string>& touch_links,
00827                                                               double padding)
00828 {
00829   const planning_models::KinematicModel::LinkModel *link = kmodel_->getLinkModel(link_name);
00830   if(link == NULL) {
00831     ROS_WARN_STREAM("No link " << link_name << " for attaching " << object_name);
00832     return false;
00833   }
00834   bodiesLock();
00835   if(link_attached_objects_.find(link_name) != link_attached_objects_.end()) {
00836     if(link_attached_objects_[link_name].find(object_name) !=
00837        link_attached_objects_[link_name].end()) {
00838       delete link_attached_objects_[link_name][object_name];
00839       link_attached_objects_[link_name].erase(object_name);
00840       kmodel_->clearLinkAttachedBodyModel(link_name, object_name);
00841     }
00842   }
00843 
00844   //the poses will be totally incorrect until they are updated with a state
00845   link_attached_objects_[link_name][object_name] = new bodies::BodyVector(shapes, poses, padding);  
00846 
00847   std::vector<std::string> modded_touch_links;
00848   
00849   //first doing group expansion of touch links
00850   for(unsigned int i = 0; i < touch_links.size(); i++) {
00851     if(kmodel_->getModelGroup(touch_links[i])) {
00852       std::vector<std::string> links = kmodel_->getModelGroup(touch_links[i])->getGroupLinkNames();
00853       modded_touch_links.insert(modded_touch_links.end(), links.begin(), links.end());
00854     } else {
00855     modded_touch_links.push_back(touch_links[i]);
00856     }
00857   }
00858 
00859   if(find(modded_touch_links.begin(), modded_touch_links.end(), link_name) == modded_touch_links.end()) {
00860     modded_touch_links.push_back(link_name);
00861   }
00862   planning_models::KinematicModel::AttachedBodyModel* ab = 
00863     new planning_models::KinematicModel::AttachedBodyModel(link, object_name,
00864                                                            poses,
00865                                                            modded_touch_links,
00866                                                            shapes);
00867   kmodel_->addAttachedBodyModel(link->getName(),ab);
00868   ode_collision_model_->lock();
00869   ode_collision_model_->updateAttachedBodies();
00870   ode_collision_model_->unlock();
00871 
00872   bodiesUnlock();
00873   return true;
00874 }
00875    
00876 bool planning_environment::CollisionModels::deleteAttachedObject(const std::string& object_id,
00877                                                                  const std::string& link_name)
00878 
00879 {
00880   bodiesLock();
00881   if(link_attached_objects_.find(link_name) != link_attached_objects_.end()) {
00882     if(link_attached_objects_[link_name].find(object_id) !=
00883        link_attached_objects_[link_name].end()) {
00884       //similarly, doing kmodel work first
00885       kmodel_->clearLinkAttachedBodyModel(link_name, object_id);
00886       delete link_attached_objects_[link_name][object_id];
00887       link_attached_objects_[link_name].erase(object_id);
00888     } else {
00889       ROS_WARN_STREAM("Link " << link_name << " has no object " << object_id << " to delete");
00890       bodiesUnlock();
00891       return false;
00892     }
00893   } else {
00894     ROS_WARN_STREAM("No link " << link_name << " for attached object delete");
00895     bodiesUnlock();
00896     return false;
00897   }
00898   ode_collision_model_->lock();
00899   ode_collision_model_->updateAttachedBodies();
00900   ode_collision_model_->unlock();
00901   bodiesUnlock();
00902   return true;
00903 }
00904 
00905 void planning_environment::CollisionModels::deleteAllAttachedObjects(const std::string& link_name)
00906 {
00907   bodiesLock();
00908   for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::iterator it = link_attached_objects_.begin();
00909       it != link_attached_objects_.end();
00910       it++) {
00911     if(link_name.empty() || it->first == "link_name") {
00912       for(std::map<std::string, bodies::BodyVector*>::iterator it2 = it->second.begin();
00913           it2 != it->second.end();
00914           it2++) {
00915         delete it2->second;
00916       }
00917     }
00918   }
00919   if(link_name.empty()) {
00920     link_attached_objects_.clear();
00921   } else {
00922     link_attached_objects_.erase(link_name);
00923   }
00924   
00925   if(link_name.empty()) {
00926     ROS_DEBUG_STREAM("Clearing all attached body models");
00927     kmodel_->clearAllAttachedBodyModels();
00928   } else {
00929     ROS_DEBUG_STREAM("Clearing all attached body models for link " << link_name);
00930     kmodel_->clearLinkAttachedBodyModels(link_name);
00931   }
00932   ode_collision_model_->lock();
00933   ode_collision_model_->updateAttachedBodies();
00934   ode_collision_model_->unlock();
00935   bodiesUnlock();
00936 }
00937 
00938 //Link pose must be in world frame
00939 bool planning_environment::CollisionModels::convertStaticObjectToAttachedObject(const std::string& object_name,
00940                                                                                 const std::string& link_name,
00941                                                                                 const tf::Transform& link_pose,
00942                                                                                 const std::vector<std::string>& touch_links)
00943 {
00944   const planning_models::KinematicModel::LinkModel *link = kmodel_->getLinkModel(link_name);
00945   if(link == NULL) {
00946     ROS_WARN_STREAM("No link " << link_name << " for attaching " << object_name);
00947     return false;
00948   }
00949   bodiesLock();
00950   if(static_object_map_.find(object_name) == static_object_map_.end()) {
00951     ROS_WARN_STREAM("No static object named " << object_name << " to convert");
00952     bodiesUnlock();
00953     return false;
00954   }
00955   link_attached_objects_[link_name][object_name] = static_object_map_[object_name];
00956   static_object_map_.erase(object_name);
00957 
00958   std::vector<std::string> modded_touch_links = touch_links;
00959   if(find(touch_links.begin(), touch_links.end(), link_name) == touch_links.end()) {
00960     modded_touch_links.push_back(link_name);
00961   }
00962 
00963   ode_collision_model_->lock();
00964   std::vector<tf::Transform> poses;
00965   std::vector<shapes::Shape*> shapes;
00966   const collision_space::EnvironmentObjects *eo = ode_collision_model_->getObjects();
00967   std::vector<std::string> ns = eo->getNamespaces();
00968   for (unsigned int i = 0 ; i < ns.size() ; ++i) {
00969     if(ns[i] == object_name) {
00970       const collision_space::EnvironmentObjects::NamespaceObjects &no = eo->getObjects(ns[i]);
00971       for(unsigned int j = 0; j < no.shape.size(); j++) {
00972         shapes.push_back(cloneShape(no.shape[j]));
00973         poses.push_back(no.shape_pose[j]);
00974       }
00975     }
00976   } 
00977 
00978   tf::Transform link_pose_inv = link_pose.inverse();
00979   //now we need to convert all these poses from the world frame into the link frame
00980   for(unsigned int i = 0; i < poses.size(); i++) {
00981     poses[i] = link_pose_inv*poses[i];
00982   }
00983 
00984   planning_models::KinematicModel::AttachedBodyModel* ab = 
00985     new planning_models::KinematicModel::AttachedBodyModel(link, object_name,
00986                                                            poses,
00987                                                            modded_touch_links,
00988                                                            shapes);
00989   kmodel_->addAttachedBodyModel(link->getName(),ab);
00990 
00991   //doing these in this order because clearObjects will take the entry
00992   //out of the allowed collision matrix and the update puts it back in
00993   ode_collision_model_->clearObjects(object_name);
00994   ode_collision_model_->updateAttachedBodies();
00995   ode_collision_model_->unlock();
00996   bodiesUnlock();
00997   return true;
00998 }
00999 
01000 bool planning_environment::CollisionModels::convertAttachedObjectToStaticObject(const std::string& object_name,
01001                                                                                 const std::string& link_name,
01002                                                                                 const tf::Transform& link_pose)
01003 {
01004   const planning_models::KinematicModel::LinkModel *link = kmodel_->getLinkModel(link_name);
01005   if(link == NULL) {
01006     ROS_WARN_STREAM("No link " << link_name << " with attached object " << object_name);
01007     return false;
01008   }
01009   bodiesLock();
01010 
01011   if(link_attached_objects_.find(link_name) == link_attached_objects_.end() ||
01012      link_attached_objects_[link_name].find(object_name) == link_attached_objects_[link_name].end()) 
01013   {
01014     ROS_WARN_STREAM("No attached body " << object_name << " attached to link " << link_name);
01015     bodiesUnlock();
01016     return false;
01017   }
01018 
01019   static_object_map_[object_name] = link_attached_objects_[link_name][object_name];
01020   link_attached_objects_[link_name].erase(object_name);
01021 
01022   const planning_models::KinematicModel::AttachedBodyModel* att = NULL;
01023   for (unsigned int i = 0 ; i < link->getAttachedBodyModels().size() ; ++i) {
01024     if(link->getAttachedBodyModels()[i]->getName() == object_name) {
01025       att = link->getAttachedBodyModels()[i];
01026       break;
01027     }
01028   }
01029 
01030   if(att == NULL) {
01031     ROS_WARN_STREAM("Something seriously out of sync");
01032     bodiesUnlock();
01033     return false;
01034   }
01035   std::vector<shapes::Shape*> shapes = shapes::cloneShapeVector(att->getShapes());
01036   std::vector<tf::Transform> poses;
01037   for(unsigned int i = 0; i < att->getAttachedBodyFixedTransforms().size(); i++) {
01038     poses.push_back(link_pose*att->getAttachedBodyFixedTransforms()[i]);
01039   }
01040   kmodel_->clearLinkAttachedBodyModel(link_name, object_name);
01041   ode_collision_model_->lock();
01042   //updating attached objects first because it clears the entry from the allowed collision matrix
01043   ode_collision_model_->updateAttachedBodies();
01044   //and then this adds it back in
01045   ode_collision_model_->addObjects(object_name, shapes, poses);  
01046   ode_collision_model_->unlock();
01047   bodiesUnlock();
01048   return true;
01049 }
01050 
01051 void planning_environment::CollisionModels::applyLinkPaddingToCollisionSpace(const std::vector<arm_navigation_msgs::LinkPadding>& link_padding) {
01052   if(link_padding.empty()) return;
01053   
01054   std::map<std::string, double> link_padding_map;
01055 
01056   for(std::vector<arm_navigation_msgs::LinkPadding>::const_iterator it = link_padding.begin();
01057       it != link_padding.end();
01058       it++) {
01059     std::vector<std::string> svec1;
01060     if(kmodel_->getModelGroup((*it).link_name)) {
01061       svec1 = kmodel_->getModelGroup((*it).link_name)->getGroupLinkNames();
01062     } else {
01063       svec1.push_back((*it).link_name);
01064     }
01065     for(std::vector<std::string>::iterator stit1 = svec1.begin();
01066         stit1 != svec1.end();
01067         stit1++) {
01068       link_padding_map[(*stit1)] = (*it).padding;
01069     }
01070   }
01071   
01072   ode_collision_model_->lock();
01073   ode_collision_model_->setAlteredLinkPadding(link_padding_map);  
01074   ode_collision_model_->unlock();
01075 }
01076 
01077 void planning_environment::CollisionModels::getCurrentLinkPadding(std::vector<arm_navigation_msgs::LinkPadding>& link_padding)
01078 {
01079   convertFromLinkPaddingMapToLinkPaddingVector(ode_collision_model_->getCurrentLinkPaddingMap(), link_padding);
01080 }
01081 
01082 bool planning_environment::CollisionModels::applyOrderedCollisionOperationsToCollisionSpace(const arm_navigation_msgs::OrderedCollisionOperations &ord, bool print) {
01083 
01084   ode_collision_model_->lock();
01085   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = ode_collision_model_->getDefaultAllowedCollisionMatrix();;
01086   ode_collision_model_->unlock();
01087 
01088   std::vector<std::string> o_strings;
01089   getCollisionObjectNames(o_strings);
01090   std::vector<std::string> a_strings;
01091   getAttachedCollisionObjectNames(a_strings);
01092 
01093   bool ok = applyOrderedCollisionOperationsListToACM(ord, o_strings, a_strings, kmodel_, acm);
01094 
01095   if(!ok) {
01096     ROS_WARN_STREAM("Bad collision operations");
01097   }
01098 
01099   if(print) {
01100     //printAllowedCollisionMatrix(acm);
01101   }
01102 
01103   ode_collision_model_->lock();
01104   ode_collision_model_->setAlteredCollisionMatrix(acm);
01105   ode_collision_model_->unlock();
01106   return true;
01107 }
01108 
01109 bool planning_environment::CollisionModels::disableCollisionsForNonUpdatedLinks(const std::string& group_name,
01110                                                                                 bool use_default)
01111 {
01112   kmodel_->sharedLock();  
01113   const planning_models::KinematicModel::JointModelGroup* joint_model_group = kmodel_->getModelGroup(group_name);
01114   collision_space::EnvironmentModel::AllowedCollisionMatrix acm;
01115   if(use_default) { 
01116     acm = ode_collision_model_->getDefaultAllowedCollisionMatrix();
01117   } else {
01118     acm = ode_collision_model_->getCurrentAllowedCollisionMatrix();
01119   }
01120   if(joint_model_group == NULL) {
01121     ROS_WARN_STREAM("No joint group " << group_name);
01122     kmodel_->sharedUnlock();  
01123     return false;
01124   }
01125 
01126   std::vector<std::string> updated_link_names = joint_model_group->getUpdatedLinkModelNames();
01127   std::map<std::string, bool> updated_link_map;
01128   for(unsigned int i = 0; i < updated_link_names.size(); i++) {
01129     updated_link_map[updated_link_names[i]] = true;
01130   }
01131   std::vector<std::string> all_link_names;
01132   kmodel_->getLinkModelNames(all_link_names);
01133 
01134   std::vector<std::string> non_group_names;
01135   for(unsigned int i = 0; i < all_link_names.size(); i++) {
01136     //some links may not be in the matrix if they don't have collision geometry
01137     if(acm.hasEntry(all_link_names[i]) && updated_link_map.find(all_link_names[i]) == updated_link_map.end()) {
01138       non_group_names.push_back(all_link_names[i]);
01139     }
01140   }
01141   std::vector<const planning_models::KinematicModel::AttachedBodyModel*> att_bodies = kmodel_->getAttachedBodyModels();
01142   for(unsigned int i = 0; i < att_bodies.size(); i++) {
01143     if(updated_link_map.find(att_bodies[i]->getAttachedLinkModel()->getName()) == updated_link_map.end()) {
01144       non_group_names.push_back(att_bodies[i]->getName());
01145     }
01146   }
01147 
01148   //now adding in static object names too
01149   for(std::map<std::string, bodies::BodyVector*>::const_iterator it = static_object_map_.begin();
01150       it != static_object_map_.end();
01151       it++) {
01152     non_group_names.push_back(it->first);
01153   }
01154 
01155   //finally, adding collision map
01156   if(acm.hasEntry(COLLISION_MAP_NAME)) {
01157     non_group_names.push_back(COLLISION_MAP_NAME);
01158   }
01159 
01160   //this allows all collisions for these links with each other
01161   if(!acm.changeEntry(non_group_names, non_group_names, true)) {
01162     ROS_INFO_STREAM("Some problem changing entries");
01163     kmodel_->sharedUnlock();  
01164     return false;
01165   }
01166   setAlteredAllowedCollisionMatrix(acm);  
01167 
01168   kmodel_->sharedUnlock();  
01169   return true;
01170 }
01171 
01172 bool planning_environment::CollisionModels::setAlteredAllowedCollisionMatrix(const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm)
01173 {
01174   ode_collision_model_->lock();
01175   ode_collision_model_->setAlteredCollisionMatrix(acm);
01176   ode_collision_model_->unlock();
01177   return true;
01178 }
01179 
01180 const collision_space::EnvironmentModel::AllowedCollisionMatrix& planning_environment::CollisionModels::getCurrentAllowedCollisionMatrix() const
01181 {
01182   return ode_collision_model_->getCurrentAllowedCollisionMatrix();
01183 }
01184 
01185 const collision_space::EnvironmentModel::AllowedCollisionMatrix& planning_environment::CollisionModels::getDefaultAllowedCollisionMatrix() const
01186 {
01187   return ode_collision_model_->getDefaultAllowedCollisionMatrix();
01188 }
01189 
01190 void planning_environment::CollisionModels::getLastCollisionMap(arm_navigation_msgs::CollisionMap& cmap) const
01191 {
01192   bodiesLock();
01193   cmap.header.frame_id = getWorldFrameId();
01194   cmap.header.stamp = ros::Time::now();
01195   cmap.boxes.clear();
01196   for(unsigned int i = 0; i < collision_map_shapes_.size(); i++) {
01197   if (collision_map_shapes_[i]->type == shapes::BOX) {
01198     const shapes::Box* box = static_cast<const shapes::Box*>(collision_map_shapes_[i]);
01199     arm_navigation_msgs::OrientedBoundingBox obb;
01200     obb.extents.x = box->size[0];
01201     obb.extents.y = box->size[1];
01202     obb.extents.z = box->size[2];
01203     const tf::Vector3 &c = collision_map_poses_[i].getOrigin();
01204     obb.center.x = c.x();
01205     obb.center.y = c.y();
01206     obb.center.z = c.z();
01207     const tf::Quaternion q = collision_map_poses_[i].getRotation();
01208     obb.angle = q.getAngle();
01209       const tf::Vector3 axis = q.getAxis();
01210       obb.axis.x = axis.x();
01211       obb.axis.y = axis.y();
01212       obb.axis.z = axis.z();
01213       cmap.boxes.push_back(obb);
01214     }
01215   }
01216   bodiesUnlock();
01217 }
01218 
01219 void planning_environment::CollisionModels::getCollisionSpaceCollisionMap(arm_navigation_msgs::CollisionMap& cmap) const
01220 {
01221   bodiesLock();
01222   ode_collision_model_->lock();
01223   cmap.header.frame_id = getWorldFrameId();
01224   cmap.header.stamp = ros::Time::now();
01225   cmap.boxes.clear();
01226   
01227   const collision_space::EnvironmentObjects::NamespaceObjects &no = ode_collision_model_->getObjects()->getObjects(COLLISION_MAP_NAME);
01228   const unsigned int n = no.shape.size();
01229   for (unsigned int i = 0 ; i < n ; ++i) {
01230     if (no.shape[i]->type == shapes::BOX) {
01231       const shapes::Box* box = static_cast<const shapes::Box*>(no.shape[i]);
01232       arm_navigation_msgs::OrientedBoundingBox obb;
01233       obb.extents.x = box->size[0];
01234       obb.extents.y = box->size[1];
01235       obb.extents.z = box->size[2];
01236       const tf::Vector3 &c = no.shape_pose[i].getOrigin();
01237       obb.center.x = c.x();
01238       obb.center.y = c.y();
01239       obb.center.z = c.z();
01240       const tf::Quaternion q = no.shape_pose[i].getRotation();
01241       obb.angle = q.getAngle();
01242       const tf::Vector3 axis = q.getAxis();
01243       obb.axis.x = axis.x();
01244       obb.axis.y = axis.y();
01245       obb.axis.z = axis.z();
01246       cmap.boxes.push_back(obb);
01247     }
01248   }
01249   ode_collision_model_->unlock();
01250   bodiesUnlock();
01251 }
01252 
01253 void planning_environment::CollisionModels::revertAllowedCollisionToDefault() {
01254   ode_collision_model_->lock();
01255   ode_collision_model_->revertAlteredCollisionMatrix();
01256   ode_collision_model_->unlock();
01257 }
01258 
01259 void planning_environment::CollisionModels::revertCollisionSpacePaddingToDefault() {
01260   ode_collision_model_->lock();
01261   ode_collision_model_->revertAlteredLinkPadding();
01262   ode_collision_model_->unlock();
01263 }
01264 
01265 void planning_environment::CollisionModels::getCollisionSpaceAllowedCollisions(arm_navigation_msgs::AllowedCollisionMatrix& ret_matrix) const {
01266 
01267   convertFromACMToACMMsg(ode_collision_model_->getCurrentAllowedCollisionMatrix(),
01268                          ret_matrix);
01269 }
01270 
01271 void planning_environment::CollisionModels::getCollisionSpaceCollisionObjects(std::vector<arm_navigation_msgs::CollisionObject> &omap) const
01272 {
01273   bodiesLock();
01274   ode_collision_model_->lock();
01275   omap.clear();
01276   const collision_space::EnvironmentObjects *eo = ode_collision_model_->getObjects();
01277   std::vector<std::string> ns = eo->getNamespaces();
01278   for (unsigned int i = 0 ; i < ns.size() ; ++i)
01279   {
01280     if (ns[i] == COLLISION_MAP_NAME)
01281       continue;
01282     const collision_space::EnvironmentObjects::NamespaceObjects &no = eo->getObjects(ns[i]);
01283     const unsigned int n = no.shape.size();
01284 
01285     arm_navigation_msgs::CollisionObject o;
01286     o.header.frame_id = getWorldFrameId();
01287     o.header.stamp = ros::Time::now();
01288     o.id = ns[i];
01289     o.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
01290     for (unsigned int j = 0 ; j < n ; ++j) {
01291       arm_navigation_msgs::Shape obj;
01292       if (constructObjectMsg(no.shape[j], obj)) {
01293         geometry_msgs::Pose pose;
01294         tf::poseTFToMsg(no.shape_pose[j], pose);
01295         o.shapes.push_back(obj);
01296         o.poses.push_back(pose);
01297       }
01298     }
01299     if(static_object_map_.find(ns[i]) == static_object_map_.end()) {
01300       ROS_WARN_STREAM("No matching internal object named " << ns[i]);
01301     } else {
01302       o.padding = static_object_map_.find(ns[i])->second->getPadding();
01303     }
01304     omap.push_back(o);
01305   }
01306   ode_collision_model_->unlock();
01307   bodiesUnlock();
01308 }
01309 
01310 void planning_environment::CollisionModels::getCollisionSpaceAttachedCollisionObjects(std::vector<arm_navigation_msgs::AttachedCollisionObject> &avec) const
01311 {
01312   avec.clear();
01313 
01314   bodiesLock();
01315   ode_collision_model_->lock();
01316 
01317   std::vector<const planning_models::KinematicModel::AttachedBodyModel*> att_vec = kmodel_->getAttachedBodyModels();
01318   for(unsigned int i = 0; i < att_vec.size(); i++) 
01319   {
01320     arm_navigation_msgs::AttachedCollisionObject ao;
01321     ao.object.header.frame_id = att_vec[i]->getAttachedLinkModel()->getName();
01322     ao.object.header.stamp = ros::Time::now();
01323     ao.link_name = att_vec[i]->getAttachedLinkModel()->getName();
01324     double attached_padd = ode_collision_model_->getCurrentLinkPadding("attached");
01325     for(unsigned int j = 0; j < att_vec[i]->getShapes().size(); j++) {
01326       arm_navigation_msgs::Shape shape;
01327       constructObjectMsg(att_vec[i]->getShapes()[j], shape, attached_padd);
01328       geometry_msgs::Pose pose;
01329       tf::poseTFToMsg(att_vec[i]->getAttachedBodyFixedTransforms()[j], pose);
01330       ao.object.shapes.push_back(shape);
01331       ao.object.poses.push_back(pose);
01332     }
01333     ao.touch_links = att_vec[i]->getTouchLinks();
01334     ao.object.id = att_vec[i]->getName();
01335     if(link_attached_objects_.find(ao.link_name) == link_attached_objects_.end()) {
01336       ROS_WARN_STREAM("No matching attached objects for link " << ao.link_name);
01337     } else if(link_attached_objects_.find(ao.link_name)->second.find(ao.object.id) == 
01338               link_attached_objects_.find(ao.link_name)->second.end()) {
01339       ROS_WARN_STREAM("No matching attached objects for link " << ao.link_name << " object " << ao.object.id);
01340     } else {
01341       ao.object.padding = link_attached_objects_.find(ao.link_name)->second.find(ao.object.id)->second->getPadding();
01342     }
01343     avec.push_back(ao);
01344   }
01345   ode_collision_model_->unlock();
01346   bodiesUnlock();
01347 }
01348 
01349 bool planning_environment::CollisionModels::isKinematicStateInCollision(const planning_models::KinematicState& state)                                                                     
01350 {
01351   ode_collision_model_->lock();
01352   ode_collision_model_->updateRobotModel(&state);
01353   bool in_coll = ode_collision_model_->isCollision();
01354   ode_collision_model_->unlock();
01355   return in_coll;
01356 }
01357 
01358 bool planning_environment::CollisionModels::isKinematicStateInSelfCollision(const planning_models::KinematicState& state)
01359 {
01360   ode_collision_model_->lock();
01361   ode_collision_model_->updateRobotModel(&state);
01362   bool in_coll = ode_collision_model_->isSelfCollision();
01363   ode_collision_model_->unlock();
01364   return in_coll;
01365 }
01366 
01367 bool planning_environment::CollisionModels::isKinematicStateInEnvironmentCollision(const planning_models::KinematicState& state)
01368 {
01369   ode_collision_model_->lock();
01370   ode_collision_model_->updateRobotModel(&state);
01371   bool in_coll = ode_collision_model_->isEnvironmentCollision();
01372   ode_collision_model_->unlock();
01373   return in_coll;
01374 }
01375 
01376 bool planning_environment::CollisionModels::isKinematicStateInObjectCollision(const planning_models::KinematicState &state, 
01377                                                                               const std::string& object_name) {
01378   ode_collision_model_->lock();
01379   ode_collision_model_->updateRobotModel(&state);
01380   bool in_coll = ode_collision_model_->isObjectRobotCollision(object_name);
01381   ode_collision_model_->unlock();
01382   return in_coll;
01383 }
01384 
01385 bool planning_environment::CollisionModels::isObjectInCollision(const std::string& object_name) {
01386   ode_collision_model_->lock();
01387   bool in_coll = ode_collision_model_->isObjectInEnvironmentCollision(object_name);
01388   ode_collision_model_->unlock();
01389   return in_coll;
01390 }
01391 
01392 void planning_environment::CollisionModels::getAllCollisionsForState(const planning_models::KinematicState& state,
01393                                                                      std::vector<arm_navigation_msgs::ContactInformation>& contacts,
01394                                                                      unsigned int num_per_pair) 
01395 {
01396   ode_collision_model_->lock();
01397   ode_collision_model_->updateRobotModel(&state);
01398   std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
01399   ros::WallTime n1 = ros::WallTime::now();
01400   ode_collision_model_->getAllCollisionContacts(coll_space_contacts,
01401                                                 num_per_pair);
01402   ros::WallTime n2 = ros::WallTime::now();
01403   ROS_DEBUG_STREAM("Got " << coll_space_contacts.size() << " collisions in " << (n2-n1).toSec());
01404   for(unsigned int i = 0; i < coll_space_contacts.size(); i++) {
01405     arm_navigation_msgs::ContactInformation contact_info;
01406     contact_info.header.frame_id = getWorldFrameId();
01407     collision_space::EnvironmentModel::Contact& contact = coll_space_contacts[i];
01408     contact_info.contact_body_1 = contact.body_name_1;
01409     contact_info.contact_body_2 = contact.body_name_2;
01410     if(contact.body_type_1 == collision_space::EnvironmentModel::LINK) {
01411       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::ROBOT_LINK;
01412     } else if(contact.body_type_1 == collision_space::EnvironmentModel::ATTACHED) {
01413       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::ATTACHED_BODY;
01414     } else {
01415       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::OBJECT;      
01416     }
01417     if(contact.body_type_2 == collision_space::EnvironmentModel::LINK) {
01418       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::ROBOT_LINK;
01419     } else if(contact.body_type_2 == collision_space::EnvironmentModel::ATTACHED) {
01420       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::ATTACHED_BODY;
01421     } else {
01422       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::OBJECT;      
01423     }
01424     contact_info.position.x = contact.pos.x();
01425     contact_info.position.y = contact.pos.y();
01426     contact_info.position.z = contact.pos.z();
01427     contacts.push_back(contact_info);
01428   }
01429   ode_collision_model_->unlock();
01430 }
01431 
01432 void planning_environment::CollisionModels::getAllEnvironmentCollisionsForObject(const std::string& object_name,  
01433                                                                                  std::vector<arm_navigation_msgs::ContactInformation>& contacts, 
01434                                                                                  unsigned int num_per_pair) {
01435   ode_collision_model_->lock();
01436   std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
01437   ode_collision_model_->getAllObjectEnvironmentCollisionContacts(object_name, coll_space_contacts, num_per_pair);
01438   for(unsigned int i = 0; i < coll_space_contacts.size(); i++) {
01439     arm_navigation_msgs::ContactInformation contact_info;
01440     contact_info.header.frame_id = getWorldFrameId();
01441     collision_space::EnvironmentModel::Contact& contact = coll_space_contacts[i];
01442     contact_info.contact_body_1 = contact.body_name_1;
01443     contact_info.contact_body_2 = contact.body_name_2;
01444     if(contact.body_type_1 == collision_space::EnvironmentModel::LINK) {
01445       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::ROBOT_LINK;
01446     } else if(contact.body_type_1 == collision_space::EnvironmentModel::ATTACHED) {
01447       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::ATTACHED_BODY;
01448     } else {
01449       contact_info.body_type_1 = arm_navigation_msgs::ContactInformation::OBJECT;      
01450     }
01451     if(contact.body_type_2 == collision_space::EnvironmentModel::LINK) {
01452       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::ROBOT_LINK;
01453     } else if(contact.body_type_2 == collision_space::EnvironmentModel::ATTACHED) {
01454       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::ATTACHED_BODY;
01455     } else {
01456       contact_info.body_type_2 = arm_navigation_msgs::ContactInformation::OBJECT;      
01457     }
01458     contact_info.position.x = contact.pos.x();
01459     contact_info.position.y = contact.pos.y();
01460     contact_info.position.z = contact.pos.z();
01461     contacts.push_back(contact_info);
01462   }
01463   ode_collision_model_->unlock();
01464 
01465 }
01466 
01467 bool planning_environment::CollisionModels::isKinematicStateValid(const planning_models::KinematicState& state,
01468                                                                   const std::vector<std::string>& joint_names,
01469                                                                   arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
01470                                                                   const arm_navigation_msgs::Constraints goal_constraints,
01471                                                                   const arm_navigation_msgs::Constraints path_constraints,
01472                                                                   bool verbose)
01473 {
01474   if(!state.areJointsWithinBounds(joint_names)) {
01475     if(verbose) {
01476       for(unsigned int j = 0; j < joint_names.size(); j++) {
01477         if(!state.isJointWithinBounds(joint_names[j])) {
01478           std::pair<double, double> bounds; 
01479           state.getJointState(joint_names[j])->getJointModel()->getVariableBounds(joint_names[j], bounds);
01480           double val = state.getJointState(joint_names[j])->getJointStateValues()[0];
01481           ROS_DEBUG_STREAM("Joint " << joint_names[j] << " out of bounds. " <<
01482                            " value: " << val << 
01483                            " low: " << bounds.first << " diff low: " << val-bounds.first << " high: " << bounds.second 
01484                            << " diff high: " << val-bounds.second);
01485         }
01486       }
01487     }
01488     error_code.val = error_code.JOINT_LIMITS_VIOLATED;
01489     return false;
01490   }
01491   if(!doesKinematicStateObeyConstraints(state, path_constraints, false)) {
01492     if(verbose) {
01493       doesKinematicStateObeyConstraints(state, path_constraints, true);
01494     }
01495     error_code.val = error_code.PATH_CONSTRAINTS_VIOLATED;
01496     return false;
01497   }
01498   if(!doesKinematicStateObeyConstraints(state, goal_constraints, false)) {
01499     if(verbose) {
01500       doesKinematicStateObeyConstraints(state, goal_constraints, true);
01501     }
01502     error_code.val = error_code.GOAL_CONSTRAINTS_VIOLATED;
01503     return false;
01504   }
01505   if(isKinematicStateInCollision(state)) {
01506     error_code.val = error_code.COLLISION_CONSTRAINTS_VIOLATED;    
01507     if(verbose) {
01508       std::vector<arm_navigation_msgs::ContactInformation> contacts;
01509       getAllCollisionsForState(state, contacts,1);
01510       if(contacts.size() == 0) {
01511         ROS_WARN_STREAM("Collision reported but no contacts");
01512       }
01513       for(unsigned int i = 0; i < contacts.size(); i++) {
01514         ROS_DEBUG_STREAM("Collision between " << contacts[i].contact_body_1 
01515                          << " and " << contacts[i].contact_body_2);
01516       }
01517     }
01518     return false;
01519   }
01520   error_code.val = error_code.SUCCESS;
01521   return true;
01522 }
01523 
01524 bool planning_environment::CollisionModels::isJointTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
01525                                                                    const trajectory_msgs::JointTrajectory &trajectory,
01526                                                                    const arm_navigation_msgs::Constraints& goal_constraints,
01527                                                                    const arm_navigation_msgs::Constraints& path_constraints,
01528                                                                    arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
01529                                                                    std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
01530                                                                    const bool evaluate_entire_trajectory)
01531 {
01532   if(planning_scene_set_) {
01533     ROS_WARN("Must revert planning scene before checking trajectory with planning scene");
01534     return false;
01535   }
01536 
01537   planning_models::KinematicState* state = setPlanningScene(planning_scene);
01538 
01539   if(state == NULL) {
01540     ROS_WARN("Planning scene invalid in isTrajectoryValid");
01541     return false;
01542   }
01543 
01544   bool ok =  isJointTrajectoryValid(*state, trajectory, goal_constraints, path_constraints, error_code, trajectory_error_codes, evaluate_entire_trajectory);
01545   revertPlanningScene(state);
01546   return ok;
01547 }
01548 
01549 
01550 bool planning_environment::CollisionModels::isJointTrajectoryValid(planning_models::KinematicState& state,
01551                                                                    const trajectory_msgs::JointTrajectory &trajectory,
01552                                                                    const arm_navigation_msgs::Constraints& goal_constraints,
01553                                                                    const arm_navigation_msgs::Constraints& path_constraints,
01554                                                                    arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
01555                                                                    std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
01556                                                                    const bool evaluate_entire_trajectory)  
01557 {
01558   error_code.val = error_code.SUCCESS;
01559   trajectory_error_codes.clear();
01560   
01561   //now we need to start evaluating the trajectory
01562   std::map<std::string, double> joint_value_map;
01563 
01564   // get the joints this trajectory is for
01565   std::vector<planning_models::KinematicState::JointState*> joints(trajectory.joint_names.size());
01566   for (unsigned int j = 0 ; j < joints.size() ; ++j)
01567   {
01568     joints[j] = state.getJointState(trajectory.joint_names[j]);
01569     if (joints[j] == NULL)
01570     {
01571       ROS_ERROR("Unknown joint '%s' found on path", trajectory.joint_names[j].c_str());
01572       error_code.val = error_code.INVALID_TRAJECTORY;
01573       return false;
01574     } else {
01575       joint_value_map[joints[j]->getName()] = 0.0;
01576     }
01577   }
01578 
01579   //now we specifically evaluate the first point of the trajectory
01580   for (unsigned int j = 0 ; j < trajectory.points.front().positions.size(); j++)
01581   {
01582     joint_value_map[trajectory.joint_names[j]] = trajectory.points.front().positions[j];
01583   }
01584   state.setKinematicState(joint_value_map);
01585 
01586   //then start state obeying path constraints
01587   if(!doesKinematicStateObeyConstraints(state, path_constraints)) {
01588     error_code.val = error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
01589     if(!evaluate_entire_trajectory) {
01590       return false;
01591     }
01592   }
01593 
01594   //next check collision
01595   ode_collision_model_->updateRobotModel(&state);
01596   if(ode_collision_model_->isCollision()) {
01597     error_code.val = error_code.START_STATE_IN_COLLISION;
01598     if(!evaluate_entire_trajectory) {
01599       return false;
01600     }
01601   }
01602 
01603   //now we make sure that the goal constraints are reasonable, at least in terms of joint_constraints
01604   //note that this only checks the position itself, not there are values within the tolerance
01605   //that don't violate the joint limits
01606   joint_value_map.clear();
01607   std::vector<std::string> goal_joint_names;
01608   for(unsigned int i = 0; i < goal_constraints.joint_constraints.size(); i++) {
01609     goal_joint_names.push_back(goal_constraints.joint_constraints[i].joint_name);
01610     joint_value_map[goal_joint_names.back()] = goal_constraints.joint_constraints[i].position;
01611   }
01612   state.setKinematicState(joint_value_map);
01613   if(!state.areJointsWithinBounds(goal_joint_names)) {
01614     error_code.val = error_code.INVALID_GOAL_JOINT_CONSTRAINTS;
01615     return false;
01616   }
01617 
01618   joint_value_map.clear();
01619   //next we check that the final point in the trajectory satisfies the goal constraints
01620   for (unsigned int j = 0 ; j < trajectory.points.back().positions.size(); j++)
01621   {
01622     joint_value_map[trajectory.joint_names[j]] = trajectory.points.back().positions[j];
01623   }
01624   state.setKinematicState(joint_value_map);
01625   if(!doesKinematicStateObeyConstraints(state, goal_constraints)) {
01626     error_code.val = error_code.GOAL_CONSTRAINTS_VIOLATED;
01627     return false;
01628   }
01629 
01630   //now we can start checking the actual 
01631   arm_navigation_msgs::Constraints emp_goal_constraints;
01632   for(unsigned int i = 0; i < trajectory.points.size(); i++) {
01633     arm_navigation_msgs::ArmNavigationErrorCodes suc;
01634     suc.val = error_code.SUCCESS;
01635     trajectory_error_codes.push_back(suc);
01636     for (unsigned int j = 0 ; j < trajectory.points[i].positions.size(); j++)
01637     {
01638       joint_value_map[trajectory.joint_names[j]] = trajectory.points[i].positions[j];
01639     }
01640     state.setKinematicState(joint_value_map);
01641 
01642     if(!isKinematicStateValid(state, trajectory.joint_names, suc, 
01643                               emp_goal_constraints, path_constraints)) {
01644       //this means we return the last error code if we are evaluating the whole trajectory
01645       error_code = suc;
01646       trajectory_error_codes.back() = suc;
01647       if(!evaluate_entire_trajectory) {
01648         return false;
01649       }
01650     }
01651     trajectory_error_codes.back() = suc;
01652   }
01653   return(error_code.val == error_code.SUCCESS);
01654 }
01655 
01656 // bool planning_environment::CollisionModels::isRobotTrajectoryValid(const arm_navigation_msgs::PlanningScene& planning_scene,
01657 //                                                                    const arm_navigation_msgs::RobotTrajectory& trajectory,
01658 //                                                                    const arm_navigation_msgs::Constraints& goal_constraints,
01659 //                                                                    const arm_navigation_msgs::Constraints& path_constraints,
01660 //                                                                    arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
01661 //                                                                    std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
01662 //                                                                    const bool evaluate_entire_trajectory)
01663 // {
01664 //   if(planning_scene_set_) {
01665 //     ROS_WARN("Must revert planning scene before checking trajectory with planning scene");
01666 //     return false;
01667 //   }
01668 
01669 //   planning_models::KinematicState* state = setPlanningScene(planning_scene);
01670 
01671 //   if(state == NULL) {
01672 //     ROS_WARN("Planning scene invalid in isTrajectoryValid");
01673 //     return false;
01674 //   }
01675 
01676 //   bool ok =  isRobotTrajectoryValid(*state, trajectory, goal_constraints, path_constraints, error_code, trajectory_error_codes, evaluate_entire_trajectory);
01677 //   revertPlanningScene(state);
01678 //   return ok;
01679 // }
01680 
01681 // bool planning_environment::CollisionModels::isRobotTrajectoryValid(planning_models::KinematicState& state,
01682 //                                                                    const arm_navigation_msgs::RobotTrajectory& trajectory,
01683 //                                                                    const arm_navigation_msgs::Constraints& goal_constraints,
01684 //                                                                    const arm_navigation_msgs::Constraints& path_constraints,
01685 //                                                                    arm_navigation_msgs::ArmNavigationErrorCodes& error_code,
01686 //                                                                    std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& trajectory_error_codes,
01687 //                                                                    const bool evaluate_entire_trajectory)
01688 // {
01689 //   //first thing to check - we can either deal with no joint_trajectory points
01690 //   //meaning that the robot state controls the positions of those links
01691 //   //or the same number of points as is in the multi_dof_trajectory
01692 //   if(trajectory.joint_trajectory.points.size() != 0 &&
01693 //      trajectory.joint_trajectory.points.size() != trajectory.multi_dof_trajectory.points.size()) {
01694 //     ROS_WARN("Invalid robot trajectory due to point number disparities");
01695 //     error_code.val = error_code.INVALID_TRAJECTORY;
01696 //     return false;
01697 //   }
01698 
01699 //   std::vector<std::string> joint_names = trajectory.joint_trajectory.joint_names;
01700 
01701 // }
01702 
01703 double planning_environment::CollisionModels::getTotalTrajectoryJointLength(planning_models::KinematicState& state,
01704                                                                           const trajectory_msgs::JointTrajectory& jtraj) const 
01705 {
01706   std::map<std::string, double> all_joint_state_values;
01707   state.getKinematicStateValues(all_joint_state_values);
01708 
01709   std::map<std::string, double> joint_positions;
01710   for(unsigned int i = 0; i < jtraj.joint_names.size(); i++) {
01711     joint_positions[jtraj.joint_names[i]] = all_joint_state_values[jtraj.joint_names[i]];
01712   }
01713   double total_path_length = 0.0;
01714   for(unsigned int i = 0; i < jtraj.points.size(); i++) {
01715     for(unsigned int j = 0; j < jtraj.joint_names.size(); j++) {
01716       joint_positions[jtraj.joint_names[j]] = jtraj.points[i].positions[j];
01717       if(i != jtraj.points.size()-1) {
01718         total_path_length += fabs(jtraj.points[i+1].positions[j]-joint_positions[jtraj.joint_names[j]]);
01719       }
01720     }
01721   }
01722   return total_path_length;
01723 }
01724 
01725 //visualization functions
01726 
01727 void planning_environment::CollisionModels::getCollisionMapAsMarkers(visualization_msgs::MarkerArray& arr,
01728                                                                      const std_msgs::ColorRGBA& color,
01729                                                                      const ros::Duration& lifetime) 
01730 {
01731   visualization_msgs::Marker mark;
01732   mark.type = visualization_msgs::Marker::CUBE_LIST;
01733   mark.color.a = 1.0;
01734   mark.lifetime = lifetime;
01735   mark.ns = "collision_map_markers";
01736   mark.id = 0;
01737   mark.header.frame_id = getWorldFrameId();
01738   mark.header.stamp = ros::Time::now();
01739   const collision_space::EnvironmentObjects::NamespaceObjects &no = ode_collision_model_->getObjects()->getObjects(COLLISION_MAP_NAME);
01740   const unsigned int n = no.shape.size();
01741   for (unsigned int i = 0 ; i < n ; ++i) {
01742     if (no.shape[i]->type == shapes::BOX) {
01743       const shapes::Box* box = static_cast<const shapes::Box*>(no.shape[i]);
01744       //first dimension of first box assumed to hold for all boxes
01745       if(i == 0) {
01746         mark.scale.x = box->size[0];
01747         mark.scale.y = box->size[0];
01748         mark.scale.z = box->size[0];
01749       }
01750       const tf::Vector3 &c = no.shape_pose[i].getOrigin();
01751       geometry_msgs::Point point;
01752       point.x = c.x();
01753       point.y = c.y();
01754       point.z = c.z();
01755       std_msgs::ColorRGBA color;      
01756       color.r = fmin(fmax(fabs(point.z)/0.5, 0.10), 1.0);
01757       color.g = fmin(fmax(fabs(point.z)/1.0, 0.20), 1.0);
01758       color.b = fmin(fmax(fabs(point.z)/1.5, 0.50), 1.0);
01759       mark.colors.push_back(color);
01760       mark.points.push_back(point);
01761     }
01762   }
01763   arr.markers.push_back(mark);
01764 }
01765 
01766 void planning_environment::CollisionModels::getAllCollisionPointMarkers(const planning_models::KinematicState& state,
01767                                                                         visualization_msgs::MarkerArray& arr,
01768                                                                         const std_msgs::ColorRGBA& color,
01769                                                                         const ros::Duration& lifetime)
01770 {
01771   std::vector<arm_navigation_msgs::ContactInformation> coll_info_vec;
01772   getAllCollisionsForState(state,coll_info_vec,1);
01773   getCollisionMarkersFromContactInformation(coll_info_vec,
01774                                             getWorldFrameId(),
01775                                             arr,
01776                                             color,
01777                                             lifetime);
01778 }
01779 
01780 void planning_environment::CollisionModels::getStaticCollisionObjectMarkers(visualization_msgs::MarkerArray& arr,
01781                                                                             const std::string& name,
01782                                                                             const std_msgs::ColorRGBA& color,
01783                                                                             const ros::Duration& lifetime) const
01784 {
01785   std::vector<arm_navigation_msgs::CollisionObject> static_objects;
01786   getCollisionSpaceCollisionObjects(static_objects);
01787   
01788   for(unsigned int i = 0; i < static_objects.size(); i++) {
01789     for(unsigned int j = 0; j < static_objects[i].shapes.size(); j++) {
01790       visualization_msgs::Marker mk;
01791       mk.header.frame_id = getWorldFrameId();
01792       mk.header.stamp = ros::Time::now();
01793       if(name.empty()) {
01794         mk.ns = static_objects[i].id;
01795       } else {
01796         mk.ns = name;
01797       }
01798       mk.id = j;
01799       mk.action = visualization_msgs::Marker::ADD;
01800       mk.pose = static_objects[i].poses[j];
01801       mk.color.a = color.a;
01802       if(mk.color.a == 0.0) {
01803         mk.color.a = 1.0;
01804       }
01805       mk.color.r = color.r;
01806       mk.color.g = color.g;
01807       mk.color.b = color.b;
01808       setMarkerShapeFromShape(static_objects[i].shapes[j], mk);
01809       mk.lifetime = lifetime;
01810       arr.markers.push_back(mk);
01811     }
01812   }
01813 }
01814 void planning_environment::CollisionModels::getAttachedCollisionObjectMarkers(const planning_models::KinematicState& state,
01815                                                                               visualization_msgs::MarkerArray& arr,
01816                                                                               const std::string& name,
01817                                                                               const std_msgs::ColorRGBA& color,
01818                                                                               const ros::Duration& lifetime,
01819                                                                               const bool show_padded,
01820                                                                               const std::vector<std::string>* link_names) const
01821 
01822 {
01823   ode_collision_model_->lock();
01824   std::vector<arm_navigation_msgs::AttachedCollisionObject> attached_objects;
01825   getCollisionSpaceAttachedCollisionObjects(attached_objects);
01826 
01827   ode_collision_model_->updateRobotModel(&state);
01828 
01829   std::map<std::string, std::vector<tf::Transform> > att_pose_map;
01830   ode_collision_model_->getAttachedBodyPoses(att_pose_map);
01831 
01832   for(unsigned int i = 0; i < attached_objects.size(); i++) {
01833     if(link_names != NULL) {
01834       bool found = false;
01835       for(unsigned int j = 0; j < link_names->size(); j++) {
01836         if(attached_objects[i].link_name == (*link_names)[j]) {
01837           found = true;
01838           break;
01839         }
01840       }
01841       if(!found) {
01842         continue;
01843       }
01844     }
01845 
01846     if(att_pose_map.find(attached_objects[i].object.id) == att_pose_map.end()) {
01847       ROS_WARN_STREAM("No collision space poses for " << attached_objects[i].object.id); 
01848       continue;
01849     }
01850     if(att_pose_map[attached_objects[i].object.id].size() !=
01851        attached_objects[i].object.shapes.size()) {
01852       ROS_WARN_STREAM("Size mismatch between poses size " << att_pose_map[attached_objects[i].object.id].size()
01853                       << " and shapes size " << attached_objects[i].object.shapes.size());
01854       continue;
01855     }
01856     for(unsigned int j = 0; j < attached_objects[i].object.shapes.size(); j++) {
01857       visualization_msgs::Marker mk;
01858       mk.header.frame_id = getWorldFrameId();
01859       mk.header.stamp = ros::Time::now();
01860       if(name.empty()) {
01861         mk.ns = attached_objects[i].object.id;
01862       } else {
01863         mk.ns = name;
01864       }
01865       mk.id = j;
01866       mk.action = visualization_msgs::Marker::ADD;
01867       tf::poseTFToMsg(att_pose_map[attached_objects[i].object.id][j], mk.pose);
01868       mk.color.a = color.a;
01869       if(mk.color.a == 0.0) {
01870         mk.color.a = 1.0;
01871       }
01872       mk.color.r = color.r;
01873       mk.color.g = color.g;
01874       mk.color.b = color.b;
01875       mk.lifetime = lifetime;
01876       setMarkerShapeFromShape(attached_objects[i].object.shapes[j], mk);
01877       arr.markers.push_back(mk);
01878     }
01879   }
01880   ode_collision_model_->unlock();
01881 }
01882 
01883 void planning_environment::CollisionModels::getPlanningSceneGivenState(const planning_models::KinematicState& state,
01884                                                                        arm_navigation_msgs::PlanningScene& planning_scene)
01885 {
01886   convertKinematicStateToRobotState(state, ros::Time::now(), getWorldFrameId(), planning_scene.robot_state);
01887   convertFromACMToACMMsg(getCurrentAllowedCollisionMatrix(), planning_scene.allowed_collision_matrix);
01888   //todo - any such thing as current allowed contacts?
01889   getCurrentLinkPadding(planning_scene.link_padding);
01890   getCollisionSpaceCollisionObjects(planning_scene.collision_objects);
01891   getCollisionSpaceAttachedCollisionObjects(planning_scene.attached_collision_objects);
01892   getCollisionSpaceCollisionMap(planning_scene.collision_map);
01893 }
01894 
01895 void planning_environment::CollisionModels::getAllCollisionSpaceObjectMarkers(const planning_models::KinematicState& state,
01896                                                                               visualization_msgs::MarkerArray& arr,
01897                                                                               const std::string& name,
01898                                                                               const std_msgs::ColorRGBA& static_color,
01899                                                                               const std_msgs::ColorRGBA& attached_color,
01900                                                                               const ros::Duration& lifetime)
01901 {
01902   getStaticCollisionObjectMarkers(arr, name, static_color, lifetime);
01903   getAttachedCollisionObjectMarkers(state, arr, name, attached_color, lifetime);
01904 }
01905 
01906 void planning_environment::CollisionModels::getRobotMarkersGivenState(const planning_models::KinematicState& state,
01907                                                                       visualization_msgs::MarkerArray& arr,
01908                                                                       const std_msgs::ColorRGBA& color,
01909                                                                       const std::string& name,
01910                                                                       const ros::Duration& lifetime,
01911                                                                       const std::vector<std::string>* names,
01912                                                                       const double scale,
01913                                                                       const bool show_collision_models) const
01914 {
01915   boost::shared_ptr<urdf::Model> robot_model = getParsedDescription();
01916 
01917   std::vector<std::string> link_names;
01918   if(names == NULL)
01919   {
01920     kmodel_->getLinkModelNames(link_names);
01921   }
01922   else
01923   {
01924     link_names = *names;
01925   }
01926 
01927   for(unsigned int i = 0; i < link_names.size(); i++)
01928   {
01929     boost::shared_ptr<const urdf::Link> urdf_link = robot_model->getLink(link_names[i]);
01930 
01931     if(!urdf_link)
01932     {
01933       ROS_INFO_STREAM("Invalid urdf name " << link_names[i]);
01934       continue;
01935     }
01936 
01937     if(show_collision_models && !urdf_link->collision && !urdf_link->visual)
01938     {
01939       continue;
01940     }
01941     if(!show_collision_models && !urdf_link->visual)
01942     {
01943       continue;
01944     }
01945     const urdf::Geometry *geom = NULL;
01946     if(show_collision_models && urdf_link->collision) {
01947       geom = urdf_link->collision->geometry.get();
01948     } else if(urdf_link->visual) {
01949       geom = urdf_link->visual->geometry.get();
01950     }
01951 
01952     if(!geom)
01953     {
01954       continue;
01955     }
01956 
01957     const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*> (geom);
01958     const urdf::Box *box = dynamic_cast<const urdf::Box*> (geom);
01959     const urdf::Sphere *sphere = dynamic_cast<const urdf::Sphere*> (geom);
01960     const urdf::Cylinder *cylinder = dynamic_cast<const urdf::Cylinder*> (geom);
01961 
01962     const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]);
01963 
01964     if(ls == NULL)
01965     {
01966       ROS_WARN_STREAM("No link state for name " << names << " though there's a mesh");
01967       continue;
01968     }
01969 
01970     visualization_msgs::Marker mark;
01971     mark.header.frame_id = getWorldFrameId();
01972     mark.header.stamp = ros::Time::now();
01973     mark.ns = name;
01974     mark.id = i;
01975     mark.lifetime = lifetime;
01976     tf::poseTFToMsg(ls->getGlobalCollisionBodyTransform(), mark.pose);
01977     mark.color = color;
01978 
01979     if(mesh)
01980     {
01981       if(mesh->filename.empty())
01982       {
01983         continue;
01984       }
01985       mark.type = mark.MESH_RESOURCE;
01986       mark.scale.x = mesh->scale.x*scale;
01987       mark.scale.y = mesh->scale.y*scale;
01988       mark.scale.z = mesh->scale.z*scale;
01989       mark.mesh_resource = mesh->filename;
01990     }
01991     else if(box)
01992     {
01993       mark.type = mark.CUBE;
01994       mark.scale.x = box->dim.x;
01995       mark.scale.y = box->dim.y;
01996       mark.scale.z = box->dim.z;
01997     }
01998     else if(cylinder)
01999     {
02000       mark.type = mark.CYLINDER;
02001       mark.scale.x = cylinder->radius;
02002       mark.scale.y = cylinder->radius;
02003       mark.scale.z = cylinder->length;
02004     } else if(sphere) {
02005       mark.type = mark.SPHERE;
02006       mark.scale.x = mark.scale.y = mark.scale.z = sphere->radius;
02007     } else {
02008       ROS_WARN_STREAM("Unknown object type for link " << link_names[i]);
02009       continue;
02010     }
02011     arr.markers.push_back(mark);
02012   }
02013 }
02014 
02015 void planning_environment::CollisionModels::getRobotPaddedMarkersGivenState(const planning_models::KinematicState& state,
02016                                                                             visualization_msgs::MarkerArray& arr,
02017                                                                             const std_msgs::ColorRGBA& color,
02018                                                                             const std::string& name,
02019                                                                             const ros::Duration& lifetime,
02020                                                                             const std::vector<std::string>* names) const
02021 {
02022   std::vector<std::string> link_names;
02023   if(names == NULL)
02024   {
02025     kmodel_->getLinkModelNames(link_names);
02026   }
02027   else
02028   {
02029     link_names = *names;
02030   }
02031   for(unsigned int i = 0; i < link_names.size(); i++) {
02032     const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]);
02033     if(ls->getLinkModel()->getLinkShape() == NULL) continue;
02034 
02035     const tf::Transform& trans = ls->getGlobalCollisionBodyTransform();
02036 
02037     visualization_msgs::Marker mark;
02038     mark.header.frame_id = getWorldFrameId();
02039     mark.header.stamp = ros::Time::now();
02040     mark.ns = name;
02041     mark.id = i;
02042     mark.color = color; 
02043     mark.lifetime = lifetime;
02044     tf::poseTFToMsg(trans, mark.pose);
02045 
02046     double padding = 0.0;
02047     if(getCurrentLinkPaddingMap().find(ls->getName()) !=
02048        getCurrentLinkPaddingMap().end()) {
02049       padding = getCurrentLinkPaddingMap().find(ls->getName())->second;
02050     }
02051     setMarkerShapeFromShape(ls->getLinkModel()->getLinkShape(), mark, padding);
02052     arr.markers.push_back(mark);
02053   }
02054 }
02055 
02056 void planning_environment::CollisionModels::getGroupAndUpdatedJointMarkersGivenState(const planning_models::KinematicState& state,
02057                                                                                      visualization_msgs::MarkerArray& arr,
02058                                                                                      const std::string& group_name, 
02059                                                                                      const std_msgs::ColorRGBA& group_color,
02060                                                                                      const std_msgs::ColorRGBA& updated_color,
02061                                                                                      const ros::Duration& lifetime) const {
02062 
02063   const planning_models::KinematicModel::JointModelGroup* jmg = kmodel_->getModelGroup(group_name);
02064   
02065   if(jmg == NULL) {
02066     ROS_WARN_STREAM("No group " << group_name << " for visualization");
02067     return;
02068   }
02069 
02070   std::vector<std::string> group_link_names = jmg->getGroupLinkNames();      
02071   getRobotMarkersGivenState(state,
02072                             arr,
02073                             group_color,
02074                             group_name,
02075                             lifetime,
02076                             &group_link_names);
02077   
02078   std::vector<std::string> updated_link_model_names = jmg->getUpdatedLinkModelNames();
02079   std::map<std::string, bool> dont_include;
02080   for(unsigned int i = 0; i < group_link_names.size(); i++) {
02081     dont_include[group_link_names[i]] = true;
02082   }
02083 
02084   std::vector<std::string> ex_list;
02085   for(unsigned int i = 0; i < updated_link_model_names.size(); i++) {
02086     if(dont_include.find(updated_link_model_names[i]) == dont_include.end()) {
02087       ex_list.push_back(updated_link_model_names[i]);
02088     }
02089   }
02090   getRobotMarkersGivenState(state,
02091                             arr,
02092                             updated_color,
02093                             group_name+"_updated_links",
02094                             lifetime,
02095                             &ex_list);
02096 
02097 }
02098 
02099 
02100 void planning_environment::CollisionModels::writePlanningSceneBag(const std::string& filename,
02101                                                                   const arm_navigation_msgs::PlanningScene& planning_scene) const
02102 {
02103   rosbag::Bag bag;
02104   bag.open(filename, rosbag::bagmode::Write);
02105   
02106   ros::Time t = planning_scene.robot_state.joint_state.header.stamp;
02107   bag.write("planning_scene", t, planning_scene);
02108   bag.close();
02109 }
02110 
02111 bool planning_environment::CollisionModels::readPlanningSceneBag(const std::string& filename,
02112                                                                  arm_navigation_msgs::PlanningScene& planning_scene) const
02113 {
02114   rosbag::Bag bag;
02115   try {
02116     bag.open(filename, rosbag::bagmode::Read);
02117   } catch(rosbag::BagException) {
02118     ROS_WARN_STREAM("Could not open bag file " << filename);
02119     return false;
02120   }
02121 
02122   std::vector<std::string> topics;
02123   topics.push_back("planning_scene");
02124 
02125   rosbag::View view(bag, rosbag::TopicQuery(topics));
02126 
02127   bool has_one = false;
02128   BOOST_FOREACH(rosbag::MessageInstance const m, view)
02129   {
02130     arm_navigation_msgs::PlanningScene::ConstPtr ps = m.instantiate<arm_navigation_msgs::PlanningScene>();
02131     if(ps != NULL) {
02132       planning_scene = *ps;
02133       has_one = true;
02134     }
02135   }    
02136   if(!has_one) {
02137     ROS_WARN_STREAM("Filename " << filename << " does not seem to contain a planning scene");
02138     return false;
02139   }
02140   return true;
02141 }
02142 
02143 bool planning_environment::CollisionModels::appendMotionPlanRequestToPlanningSceneBag(const std::string& filename,
02144                                                                                       const std::string& topic_name,
02145                                                                                       const arm_navigation_msgs::MotionPlanRequest& req)
02146 {
02147   rosbag::Bag bag;
02148   try {
02149     bag.open(filename, rosbag::bagmode::Append);
02150   } catch(rosbag::BagException) {
02151     ROS_WARN_STREAM("Could not append to bag file " << filename);
02152     return false;
02153   }
02154   ros::Time t = req.start_state.joint_state.header.stamp;
02155   bag.write(topic_name, ros::Time::now(), req);
02156   bag.close();
02157   return true;
02158 }
02159 
02160 bool planning_environment::CollisionModels::loadMotionPlanRequestsInPlanningSceneBag(const std::string& filename,
02161                                                                                      const std::string& topic_name,
02162                                                                                      std::vector<arm_navigation_msgs::MotionPlanRequest>& motion_plan_reqs){
02163   rosbag::Bag bag;
02164   try {
02165     bag.open(filename, rosbag::bagmode::Read);
02166   } catch(rosbag::BagException) {
02167     ROS_WARN_STREAM("Could not open bag file " << filename);
02168     return false;
02169   }
02170 
02171   std::vector<std::string> topics;
02172   topics.push_back(topic_name);
02173 
02174   rosbag::View view(bag, rosbag::TopicQuery(topics));
02175 
02176   BOOST_FOREACH(rosbag::MessageInstance const m, view)
02177   {
02178     arm_navigation_msgs::MotionPlanRequest::ConstPtr mpr = m.instantiate<arm_navigation_msgs::MotionPlanRequest>();
02179     if(mpr != NULL) {
02180       motion_plan_reqs.push_back(*mpr);
02181     }
02182   }    
02183   if(motion_plan_reqs.size() == 0) {
02184     ROS_WARN_STREAM("No MotionPlanRequest messages with topic name " << topic_name << " in " << filename);
02185     return false;
02186   }
02187   return true;  
02188 }
02189 
02190 bool planning_environment::CollisionModels::loadJointTrajectoriesInPlanningSceneBag(const std::string& filename,
02191                                                                                     const std::string& topic_name,
02192                                                                                     std::vector<trajectory_msgs::JointTrajectory>& traj_vec){
02193   rosbag::Bag bag;
02194   try {
02195     bag.open(filename, rosbag::bagmode::Read);
02196   } catch(rosbag::BagException) {
02197     ROS_WARN_STREAM("Could not open bag file " << filename);
02198     return false;
02199   }
02200 
02201   std::vector<std::string> topics;
02202   topics.push_back(topic_name);
02203 
02204   rosbag::View view(bag, rosbag::TopicQuery(topics));
02205 
02206   BOOST_FOREACH(rosbag::MessageInstance const m, view)
02207   {
02208     trajectory_msgs::JointTrajectory::ConstPtr jt = m.instantiate<trajectory_msgs::JointTrajectory>();
02209     if(jt != NULL) {
02210       traj_vec.push_back(*jt);
02211     }
02212   }    
02213   if(traj_vec.size() == 0) {
02214     ROS_WARN_STREAM("No JointTrajectory messages with topic name " << topic_name << " in " << filename);
02215     return false;
02216   }
02217   return true;  
02218 }
02219 
02220 bool planning_environment::CollisionModels::appendJointTrajectoryToPlanningSceneBag(const std::string& filename,
02221                                                                                     const std::string& topic_name,
02222                                                                                     const trajectory_msgs::JointTrajectory& traj)
02223 {
02224   rosbag::Bag bag;
02225   try {
02226     bag.open(filename, rosbag::bagmode::Append);
02227   } catch(rosbag::BagException) {
02228     ROS_WARN_STREAM("Could not append to bag file " << filename);
02229     return false;
02230   }
02231   bag.write(topic_name, ros::Time::now(), traj);
02232   bag.close();
02233   return true;
02234 }
02235 


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43