collision_common_distance_field.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 
00035 /* Author: E. Gil Jones */
00036 
00037 #include <moveit/collision_distance_field/collision_common_distance_field.h>
00038 #include <ros/console.h>
00039 #include <boost/thread/mutex.hpp>
00040 #include <eigen_conversions/eigen_msg.h>
00041 
00042 namespace collision_detection
00043 {
00044 struct BodyDecompositionCache
00045 {
00046   BodyDecompositionCache() : clean_count_(0)
00047   {
00048   }
00049   static const unsigned int MAX_CLEAN_COUNT = 100;
00050   std::map<boost::weak_ptr<const shapes::Shape>, BodyDecompositionConstPtr> map_;
00051   unsigned int clean_count_;
00052   boost::mutex lock_;
00053 };
00054 
00055 BodyDecompositionCache& getBodyDecompositionCache()
00056 {
00057   static BodyDecompositionCache cache;
00058   return cache;
00059 }
00060 
00061 BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr& shape, double resolution)
00062 {
00063   // TODO - deal with changing resolution?
00064   BodyDecompositionCache& cache = getBodyDecompositionCache();
00065   boost::weak_ptr<const shapes::Shape> wptr(shape);
00066   {
00067     boost::mutex::scoped_lock slock(cache.lock_);
00068     std::map<boost::weak_ptr<const shapes::Shape>, BodyDecompositionConstPtr>::const_iterator cache_it =
00069         cache.map_.find(wptr);
00070     if (cache_it != cache.map_.end())
00071     {
00072       return cache_it->second;
00073     }
00074   }
00075 
00076   BodyDecompositionConstPtr bdcp(new BodyDecomposition(shape, resolution));
00077   {
00078     boost::mutex::scoped_lock slock(cache.lock_);
00079     cache.map_[wptr] = bdcp;
00080     cache.clean_count_++;
00081     return bdcp;
00082   }
00083   // TODO - clean cache
00084 }
00085 
00086 PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj,
00087                                                                           double resolution)
00088 {
00089   PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
00090   for (unsigned int i = 0; i < obj.shapes_.size(); i++)
00091   {
00092     PosedBodyPointDecompositionPtr pbd(
00093         new PosedBodyPointDecomposition(getBodyDecompositionCacheEntry(obj.shapes_[i], resolution)));
00094     ret->addToVector(pbd);
00095     ret->updatePose(ret->getSize() - 1, obj.shape_poses_[i]);
00096   }
00097   return ret;
00098 }
00099 
00100 PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att,
00101                                                                          double resolution)
00102 {
00103   PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector());
00104   for (unsigned int i = 0; i < att->getShapes().size(); i++)
00105   {
00106     PosedBodySphereDecompositionPtr pbd(
00107         new PosedBodySphereDecomposition(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution)));
00108     pbd->updatePose(att->getGlobalCollisionBodyTransforms()[i]);
00109     ret->addToVector(pbd);
00110   }
00111   return ret;
00112 }
00113 
00114 PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att,
00115                                                                        double resolution)
00116 {
00117   PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
00118   for (unsigned int i = 0; i < att->getShapes().size(); i++)
00119   {
00120     PosedBodyPointDecompositionPtr pbd(
00121         new PosedBodyPointDecomposition(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution)));
00122     ret->addToVector(pbd);
00123     ret->updatePose(ret->getSize() - 1, att->getGlobalCollisionBodyTransforms()[i]);
00124   }
00125   return ret;
00126 }
00127 
00128 void getBodySphereVisualizationMarkers(boost::shared_ptr<const collision_detection::GroupStateRepresentation>& gsr,
00129                                        std::string reference_frame, visualization_msgs::MarkerArray& body_marker_array)
00130 {
00131   // creating namespaces
00132   std::string robot_ns = gsr->dfce_->group_name_ + "_sphere_decomposition";
00133   std::string attached_ns = "attached_sphere_decomposition";
00134 
00135   // creating colors
00136   std_msgs::ColorRGBA robot_color;
00137   robot_color.r = 0;
00138   robot_color.b = 0.8f;
00139   robot_color.g = 0;
00140   robot_color.a = 0.5;
00141 
00142   std_msgs::ColorRGBA attached_color;
00143   attached_color.r = 1;
00144   attached_color.g = 1;
00145   attached_color.b = 0;
00146   attached_color.a = 0.5;
00147 
00148   // creating sphere marker
00149   visualization_msgs::Marker sphere_marker;
00150   sphere_marker.header.frame_id = reference_frame;
00151   sphere_marker.header.stamp = ros::Time(0);
00152   sphere_marker.ns = robot_ns;
00153   sphere_marker.id = 0;
00154   sphere_marker.type = visualization_msgs::Marker::SPHERE;
00155   sphere_marker.action = visualization_msgs::Marker::ADD;
00156   sphere_marker.pose.orientation.x = 0;
00157   sphere_marker.pose.orientation.y = 0;
00158   sphere_marker.pose.orientation.z = 0;
00159   sphere_marker.pose.orientation.w = 1;
00160   sphere_marker.color = robot_color;
00161   sphere_marker.lifetime = ros::Duration(0);
00162 
00163   const moveit::core::RobotState& state = *(gsr->dfce_->state_);
00164   unsigned int id = 0;
00165   for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
00166   {
00167     const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
00168     if (gsr->dfce_->link_has_geometry_[i])
00169     {
00170       gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
00171 
00172       collision_detection::PosedBodySphereDecompositionConstPtr sphere_representation =
00173           gsr->link_body_decompositions_[i];
00174       for (unsigned int j = 0; j < sphere_representation->getCollisionSpheres().size(); j++)
00175       {
00176         tf::pointEigenToMsg(sphere_representation->getSphereCenters()[j], sphere_marker.pose.position);
00177         sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
00178             sphere_representation->getCollisionSpheres()[j].radius_;
00179         sphere_marker.id = id;
00180         id++;
00181 
00182         body_marker_array.markers.push_back(sphere_marker);
00183       }
00184     }
00185   }
00186 
00187   sphere_marker.ns = attached_ns;
00188   sphere_marker.color = attached_color;
00189   for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); i++)
00190   {
00191     int link_index = gsr->dfce_->attached_body_link_state_indices_[i];
00192     const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
00193     if (!att)
00194     {
00195       ROS_WARN("Attached body '%s' was not found, skipping sphere "
00196                "decomposition visualization",
00197                gsr->dfce_->attached_body_names_[i].c_str());
00198       continue;
00199     }
00200 
00201     if (gsr->attached_body_decompositions_[i]->getSize() != att->getShapes().size())
00202     {
00203       ROS_WARN("Attached body size discrepancy");
00204       continue;
00205     }
00206 
00207     for (unsigned int j = 0; j < att->getShapes().size(); j++)
00208     {
00209       PosedBodySphereDecompositionVectorPtr sphere_decp = gsr->attached_body_decompositions_[i];
00210       sphere_decp->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
00211 
00212       tf::pointEigenToMsg(sphere_decp->getSphereCenters()[j], sphere_marker.pose.position);
00213       sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
00214           sphere_decp->getCollisionSpheres()[j].radius_;
00215       sphere_marker.id = id;
00216       body_marker_array.markers.push_back(sphere_marker);
00217       id++;
00218     }
00219   }
00220 }
00221 }


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Wed Jun 19 2019 19:24:03