shape_mask.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 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: Ioan Sucan */
00036 
00037 #include <moveit/point_containment_filter/shape_mask.h>
00038 #include <geometric_shapes/body_operations.h>
00039 #include <ros/console.h>
00040 #include <sensor_msgs/point_cloud2_iterator.h>
00041 
00042 point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback)
00043   : transform_callback_(transform_callback), next_handle_(1), min_handle_(1)
00044 {
00045 }
00046 
00047 point_containment_filter::ShapeMask::~ShapeMask()
00048 {
00049   freeMemory();
00050 }
00051 
00052 void point_containment_filter::ShapeMask::freeMemory()
00053 {
00054   for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it)
00055     delete it->body;
00056   bodies_.clear();
00057 }
00058 
00059 void point_containment_filter::ShapeMask::setTransformCallback(const TransformCallback& transform_callback)
00060 {
00061   boost::mutex::scoped_lock _(shapes_lock_);
00062   transform_callback_ = transform_callback;
00063 }
00064 
00065 point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape(const shapes::ShapeConstPtr& shape,
00066                                                                                     double scale, double padding)
00067 {
00068   boost::mutex::scoped_lock _(shapes_lock_);
00069   SeeShape ss;
00070   ss.body = bodies::createBodyFromShape(shape.get());
00071   if (ss.body)
00072   {
00073     ss.body->setScale(scale);
00074     ss.body->setPadding(padding);
00075     ss.volume = ss.body->computeVolume();
00076     ss.handle = next_handle_;
00077     std::pair<std::set<SeeShape, SortBodies>::iterator, bool> insert_op = bodies_.insert(ss);
00078     if (!insert_op.second)
00079       ROS_ERROR("Internal error in management of bodies in ShapeMask. This is a serious error.");
00080     used_handles_[next_handle_] = insert_op.first;
00081   }
00082   else
00083     return 0;
00084 
00085   ShapeHandle ret = next_handle_;
00086   const std::size_t sz = min_handle_ + bodies_.size() + 1;
00087   for (std::size_t i = min_handle_; i < sz; ++i)
00088     if (used_handles_.find(i) == used_handles_.end())
00089     {
00090       next_handle_ = i;
00091       break;
00092     }
00093   min_handle_ = next_handle_;
00094 
00095   return ret;
00096 }
00097 
00098 void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle)
00099 {
00100   boost::mutex::scoped_lock _(shapes_lock_);
00101   std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
00102   if (it != used_handles_.end())
00103   {
00104     delete it->second->body;
00105     bodies_.erase(it->second);
00106     used_handles_.erase(it);
00107     min_handle_ = handle;
00108   }
00109   else
00110     ROS_ERROR("Unable to remove shape handle %u", handle);
00111 }
00112 
00113 void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in,
00114                                                           const Eigen::Vector3d& sensor_origin,
00115                                                           const double min_sensor_dist, const double max_sensor_dist,
00116                                                           std::vector<int>& mask)
00117 {
00118   boost::mutex::scoped_lock _(shapes_lock_);
00119   const unsigned int np = data_in.data.size() / data_in.point_step;
00120   mask.resize(np);
00121 
00122   if (bodies_.empty())
00123     std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00124   else
00125   {
00126     Eigen::Affine3d tmp;
00127     bspheres_.resize(bodies_.size());
00128     std::size_t j = 0;
00129     for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it)
00130     {
00131       if (transform_callback_(it->handle, tmp))
00132       {
00133         it->body->setPose(tmp);
00134         it->body->computeBoundingSphere(bspheres_[j++]);
00135       }
00136     }
00137 
00138     // compute a sphere that bounds the entire robot
00139     bodies::BoundingSphere bound;
00140     bodies::mergeBoundingSpheres(bspheres_, bound);
00141     const double radiusSquared = bound.radius * bound.radius;
00142 
00143     // we now decide which points we keep
00144     sensor_msgs::PointCloud2ConstIterator<float> iter_x(data_in, "x");
00145     sensor_msgs::PointCloud2ConstIterator<float> iter_y(data_in, "y");
00146     sensor_msgs::PointCloud2ConstIterator<float> iter_z(data_in, "z");
00147 
00148     // Cloud iterators are not incremented in the for loop, because of the pragma
00149     // Comment out below parallelization as it can result in very high CPU consumption
00150     //#pragma omp parallel for schedule(dynamic)
00151     for (int i = 0; i < (int)np; ++i)
00152     {
00153       Eigen::Vector3d pt = Eigen::Vector3d(*(iter_x + i), *(iter_y + i), *(iter_z + i));
00154       double d = pt.norm();
00155       int out = OUTSIDE;
00156       if (d < min_sensor_dist || d > max_sensor_dist)
00157         out = CLIP;
00158       else if ((bound.center - pt).squaredNorm() < radiusSquared)
00159         for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
00160           if (it->body->containsPoint(pt))
00161             out = INSIDE;
00162       mask[i] = out;
00163     }
00164   }
00165 }
00166 
00167 int point_containment_filter::ShapeMask::getMaskContainment(const Eigen::Vector3d& pt) const
00168 {
00169   boost::mutex::scoped_lock _(shapes_lock_);
00170 
00171   int out = OUTSIDE;
00172   for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
00173     if (it->body->containsPoint(pt))
00174       out = INSIDE;
00175   return out;
00176 }
00177 
00178 int point_containment_filter::ShapeMask::getMaskContainment(double x, double y, double z) const
00179 {
00180   return getMaskContainment(Eigen::Vector3d(x, y, z));
00181 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10