planning_environment: planning_environment: planning

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/monitors/planning_monitor.h"
00038 //#include "planning_environment/util/kinematic_state_constraint_evaluator.h"
00039 #include <boost/scoped_ptr.hpp>
00040 //#include <arm_navigation_msgs/DisplayTrajectory.h>
00041 #include "planning_environment/models/model_utils.h"
00042 
00043 void planning_environment::PlanningMonitor::loadParams(void)
00044 {
00045 }
00046 
00047 bool planning_environment::PlanningMonitor::getCompletePlanningScene(const arm_navigation_msgs::PlanningScene& planning_diff,
00048                                                                      const arm_navigation_msgs::OrderedCollisionOperations& ordered_collision_operations,
00049                                                                      arm_navigation_msgs::PlanningScene& planning_scene) const{
00050   {
00051     //indenting because we only need the state in here
00052     //creating state    
00053     planning_models::KinematicState set_state(getKinematicModel());    
00054     //setting state to current values
00055     setStateValuesFromCurrentValues(set_state);
00056     //supplementing with state_diff
00057     setRobotStateAndComputeTransforms(planning_diff.robot_state, set_state);
00058     
00059     //now complete robot state is populated
00060     convertKinematicStateToRobotState(set_state,
00061                                       last_joint_state_update_,
00062                                       cm_->getWorldFrameId(),
00063                                       planning_scene.robot_state);
00064   }
00065 
00066   //getting full list of tf transforms not associated with the robot's body
00067   getAllFixedFrameTransforms(planning_scene.fixed_frame_transforms);
00068   
00069   //getting all the stuff from the current collision space
00070   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCollisionSpace()->getDefaultAllowedCollisionMatrix();
00071   if(planning_diff.allowed_collision_matrix.link_names.size() > 0) {
00072     acm = convertFromACMMsgToACM(planning_diff.allowed_collision_matrix);
00073   }
00074 
00075   //first we deal with collision object diffs
00076   cm_->getCollisionSpaceCollisionObjects(planning_scene.collision_objects);
00077 
00078   for(unsigned int i = 0; i < planning_scene.collision_objects.size(); i++) {
00079     if(!acm.hasEntry(planning_scene.collision_objects[i].id)) {
00080       ROS_ERROR_STREAM("Sanity check failing - no entry in acm for collision space object " << planning_scene.collision_objects[i].id);
00081     } 
00082   }
00083   
00084   cm_->getLastCollisionMap(planning_scene.collision_map);
00085 
00086   //probably haven't gotten another collision map yet after a clear
00087   if(planning_scene.collision_map.boxes.size() > 0 && !acm.hasEntry(COLLISION_MAP_NAME)) {
00088     ROS_INFO_STREAM("Adding entry for collision map");
00089     acm.addEntry(COLLISION_MAP_NAME, false);
00090   }
00091 
00092   //now attached objects
00093   cm_->getCollisionSpaceAttachedCollisionObjects(planning_scene.attached_collision_objects);
00094 
00095   for(unsigned int i = 0; i < planning_scene.attached_collision_objects.size(); i++) {
00096     if(!acm.hasEntry(planning_scene.attached_collision_objects[i].object.id)) {
00097       ROS_ERROR_STREAM("Sanity check failing - no entry in acm for attached collision space object " << planning_scene.attached_collision_objects[i].object.id);
00098     } 
00099   }
00100 
00101   std::map<std::string, double> cur_link_padding = cm_->getCollisionSpace()->getCurrentLinkPaddingMap();
00102 
00103   for(unsigned int i = 0; i < planning_diff.collision_objects.size(); i++) {
00104     std::string object_name = planning_diff.collision_objects[i].id;
00105     if(object_name == "all") {
00106       if(planning_diff.collision_objects[i].operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
00107         for(unsigned int j = 0; j < planning_scene.collision_objects.size(); i++) {
00108           acm.removeEntry(planning_scene.collision_objects[j].id);
00109         }
00110         planning_scene.collision_objects.clear();
00111         continue;
00112       } else {
00113         ROS_WARN_STREAM("No other operation than remove permitted for all");
00114         return false;
00115       }
00116     }
00117     bool already_have = false;
00118     std::vector<arm_navigation_msgs::CollisionObject>::iterator it = planning_scene.collision_objects.begin();
00119     while(it != planning_scene.collision_objects.end()) {
00120       if((*it).id == object_name) {
00121         already_have = true;
00122         break;
00123       }
00124       it++;
00125     }
00126     if(planning_diff.collision_objects[i].operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
00127       if(!already_have) {
00128         ROS_WARN_STREAM("Diff remove specified for object " << object_name << " which we don't seem to have");
00129         continue;
00130       }
00131       acm.removeEntry(object_name);
00132       planning_scene.collision_objects.erase(it);
00133     } else {
00134       //must be an add
00135       if(already_have) {
00136         //if we already have it don't need to add to the matrix
00137         planning_scene.collision_objects.erase(it);
00138       } else {
00139         acm.addEntry(object_name, false);
00140       }
00141       planning_scene.collision_objects.push_back(planning_diff.collision_objects[i]);
00142     }
00143   }
00144   
00145 
00146   for(unsigned int i = 0; i < planning_diff.attached_collision_objects.size(); i++) {
00147     std::string link_name = planning_diff.attached_collision_objects[i].link_name;
00148     std::string object_name = planning_diff.attached_collision_objects[i].object.id;
00149     if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT ||
00150        planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT) {
00151       ROS_WARN_STREAM("Object replacement not supported during diff");
00152     }
00153     if(link_name == "all") {
00154       if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
00155         for(unsigned int j = 0; j < planning_scene.attached_collision_objects.size(); i++) {
00156           acm.removeEntry(planning_scene.attached_collision_objects[j].object.id);
00157         }
00158         planning_scene.attached_collision_objects.clear();
00159         continue;
00160       } else {
00161         ROS_WARN_STREAM("No other operation than remove permitted for all");
00162         return false;        
00163       }
00164     } else {
00165       if(object_name == "all") {
00166         if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
00167           std::vector<arm_navigation_msgs::AttachedCollisionObject>::iterator it = planning_scene.attached_collision_objects.begin();
00168           while(it != planning_scene.attached_collision_objects.end()) {
00169             if((*it).link_name == link_name) {
00170               acm.removeEntry((*it).object.id);
00171               it == planning_scene.attached_collision_objects.erase(it);
00172             } else {
00173               it++;
00174             }
00175           }
00176         } else {
00177           ROS_WARN_STREAM("No other operation than remove permitted for all");
00178           return false;
00179         }
00180         continue;
00181       }
00182       bool already_have = false;
00183       std::vector<arm_navigation_msgs::AttachedCollisionObject>::iterator it = planning_scene.attached_collision_objects.begin();
00184       while(it != planning_scene.attached_collision_objects.end()) {
00185         if((*it).link_name == link_name) {
00186           if((*it).object.id == object_name) {
00187             already_have = true;
00188             break;
00189           }
00190         }
00191         it++;
00192       }
00193       if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
00194         if(!already_have) {
00195           ROS_WARN_STREAM("Diff remove specified for object " << object_name << " which we don't seem to have");          
00196           return false;
00197         }
00198         acm.removeEntry((*it).object.id);
00199         planning_scene.attached_collision_objects.erase(it);
00200       } else {
00201         //must be an add
00202         if(already_have) {
00203           planning_scene.attached_collision_objects.erase(it);
00204         } else {
00205           acm.addEntry(planning_diff.attached_collision_objects[i].object.id, false);
00206         }
00207         acm.changeEntry(planning_diff.attached_collision_objects[i].object.id, planning_diff.attached_collision_objects[i].link_name, true);
00208         std::vector<std::string> modded_touch_links; 
00209         for(unsigned int j = 0; j < planning_diff.attached_collision_objects[i].touch_links.size(); j++) {
00210           if(cm_->getKinematicModel()->getModelGroup(planning_diff.attached_collision_objects[i].touch_links[j])) {
00211             std::vector<std::string> links = cm_->getKinematicModel()->getModelGroup(planning_diff.attached_collision_objects[i].touch_links[j])->getGroupLinkNames();
00212             modded_touch_links.insert(modded_touch_links.end(), links.begin(), links.end());
00213           } else {
00214             modded_touch_links.push_back(planning_diff.attached_collision_objects[i].touch_links[j]);
00215           }
00216         }
00217         for(unsigned int j = 0; j < modded_touch_links.size(); j++) {
00218           acm.changeEntry(planning_diff.attached_collision_objects[i].object.id, modded_touch_links[j], true);
00219         }
00220         planning_scene.attached_collision_objects.push_back(planning_diff.attached_collision_objects[i]);
00221       }
00222     }
00223   }
00224 
00225   for(unsigned int i = 0; i < planning_diff.link_padding.size(); i++) {
00226     //note - for things like attached objects this might just add them to the mix, but that's fine
00227     cur_link_padding[planning_diff.link_padding[i].link_name] = planning_diff.link_padding[i].padding;
00228   }
00229   convertFromLinkPaddingMapToLinkPaddingVector(cur_link_padding, planning_scene.link_padding);
00230 
00231   //now we need to apply the allowed collision operations to the modified ACM
00232   std::vector<std::string> o_strings;
00233   for(unsigned int i = 0; i < planning_scene.collision_objects.size(); i++) {
00234     o_strings.push_back(planning_scene.collision_objects[i].id);
00235   }
00236 
00237   std::vector<std::string> a_strings;
00238   for(unsigned int i = 0; i < planning_scene.attached_collision_objects.size(); i++) {
00239     a_strings.push_back(planning_scene.attached_collision_objects[i].object.id);
00240   }
00241 
00242   applyOrderedCollisionOperationsListToACM(ordered_collision_operations,
00243                                            o_strings,
00244                                            a_strings,
00245                                            cm_->getKinematicModel(),
00246                                            acm);
00247   
00248   convertFromACMToACMMsg(acm, planning_scene.allowed_collision_matrix);
00249 
00250   planning_scene.allowed_contacts = planning_diff.allowed_contacts;
00251 
00252   return true;
00253 }   
00254 
00255 void planning_environment::PlanningMonitor::getAllFixedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transform_vec) const
00256 {
00257   transform_vec.clear();
00258 
00259   std::vector<std::string> all_frame_names;
00260   tf_->getFrameStrings(all_frame_names);
00261   //TODO - doesn't cope with tf namespaces
00262   //takes out leading slashes
00263   for(unsigned int i = 0; i < all_frame_names.size(); i++) {
00264     if(!all_frame_names[i].empty() && all_frame_names[i][0] == '/') {
00265       all_frame_names[i].erase(all_frame_names[i].begin());
00266     }
00267   }
00268   //the idea here is that we need to figure out how to take poses from other frames
00269   //and transform them into the fixed frame. So we want to get the transform
00270   //that goes from the frame to the identity of the world frame and take the inverse
00271   for(unsigned int i = 0; i < all_frame_names.size(); i++) {
00272     if(all_frame_names[i] != cm_->getWorldFrameId() && 
00273        !cm_->getKinematicModel()->hasLinkModel(all_frame_names[i])) {
00274       ROS_DEBUG_STREAM("Adding fixed frame transform for frame " << all_frame_names[i]);
00275       geometry_msgs::PoseStamped ident_pose;
00276       ident_pose.header.frame_id = cm_->getWorldFrameId();
00277       ident_pose.pose.orientation.w = 1.0;
00278       std::string err_string;
00279       if (tf_->getLatestCommonTime(cm_->getWorldFrameId(), all_frame_names[i], ident_pose.header.stamp, &err_string) != tf::NO_ERROR) {
00280         ROS_WARN_STREAM("No transform whatsoever available between " << all_frame_names[i] << " and fixed frame" << cm_->getWorldFrameId());
00281         continue;
00282       }
00283       geometry_msgs::PoseStamped trans_pose;
00284       try {
00285         tf_->transformPose(all_frame_names[i], ident_pose, trans_pose);
00286       } catch(tf::TransformException& ex) {
00287         //just not going to cache this one
00288         //ROS_WARN_STREAM("Unable to transform object from frame " << all_frame_names[i] << " to fixed frame " 
00289         //                 << cm_->getWorldFrameId() << " tf error is " << ex.what());
00290         continue;
00291       }
00292       geometry_msgs::TransformStamped f;
00293       f.header = ident_pose.header;
00294       f.child_frame_id = all_frame_names[i];
00295       f.transform.translation.x = trans_pose.pose.position.x;
00296       f.transform.translation.y = trans_pose.pose.position.y;
00297       f.transform.translation.z = trans_pose.pose.position.z;
00298       f.transform.rotation = trans_pose.pose.orientation;
00299       transform_vec.push_back(f);
00300     }
00301   }
00302 }
planning_monitor.cpp
