pointcloud_octomap_updater.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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: Jon Binney, Ioan Sucan */
00036 
00037 #include <cmath>
00038 #include <moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h>
00039 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00040 #include <message_filters/subscriber.h>
00041 #include <sensor_msgs/point_cloud2_iterator.h>
00042 #include <XmlRpcException.h>
00043 
00044 namespace occupancy_map_monitor
00045 {
00046 PointCloudOctomapUpdater::PointCloudOctomapUpdater()
00047   : OccupancyMapUpdater("PointCloudUpdater")
00048   , private_nh_("~")
00049   , scale_(1.0)
00050   , padding_(0.0)
00051   , max_range_(std::numeric_limits<double>::infinity())
00052   , point_subsample_(1)
00053   , point_cloud_subscriber_(NULL)
00054   , point_cloud_filter_(NULL)
00055 {
00056 }
00057 
00058 PointCloudOctomapUpdater::~PointCloudOctomapUpdater()
00059 {
00060   stopHelper();
00061 }
00062 
00063 bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
00064 {
00065   try
00066   {
00067     if (!params.hasMember("point_cloud_topic"))
00068       return false;
00069     point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
00070 
00071     readXmlParam(params, "max_range", &max_range_);
00072     readXmlParam(params, "padding_offset", &padding_);
00073     readXmlParam(params, "padding_scale", &scale_);
00074     readXmlParam(params, "point_subsample", &point_subsample_);
00075     if (params.hasMember("filtered_cloud_topic"))
00076       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
00077   }
00078   catch (XmlRpc::XmlRpcException& ex)
00079   {
00080     ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
00081     return false;
00082   }
00083 
00084   return true;
00085 }
00086 
00087 bool PointCloudOctomapUpdater::initialize()
00088 {
00089   tf_ = monitor_->getTFClient();
00090   shape_mask_.reset(new point_containment_filter::ShapeMask());
00091   shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
00092   if (!filtered_cloud_topic_.empty())
00093     filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
00094   return true;
00095 }
00096 
00097 void PointCloudOctomapUpdater::start()
00098 {
00099   if (point_cloud_subscriber_)
00100     return;
00101   /* subscribe to point cloud topic using tf filter*/
00102   point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 5);
00103   if (tf_ && !monitor_->getMapFrame().empty())
00104   {
00105     point_cloud_filter_ =
00106         new tf::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_, monitor_->getMapFrame(), 5);
00107     point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
00108     ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
00109              point_cloud_filter_->getTargetFramesString().c_str());
00110   }
00111   else
00112   {
00113     point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
00114     ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
00115   }
00116 }
00117 
00118 void PointCloudOctomapUpdater::stopHelper()
00119 {
00120   delete point_cloud_filter_;
00121   delete point_cloud_subscriber_;
00122 }
00123 
00124 void PointCloudOctomapUpdater::stop()
00125 {
00126   stopHelper();
00127   point_cloud_filter_ = NULL;
00128   point_cloud_subscriber_ = NULL;
00129 }
00130 
00131 ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& shape)
00132 {
00133   ShapeHandle h = 0;
00134   if (shape_mask_)
00135     h = shape_mask_->addShape(shape, scale_, padding_);
00136   else
00137     ROS_ERROR("Shape filter not yet initialized!");
00138   return h;
00139 }
00140 
00141 void PointCloudOctomapUpdater::forgetShape(ShapeHandle handle)
00142 {
00143   if (shape_mask_)
00144     shape_mask_->removeShape(handle);
00145 }
00146 
00147 bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Affine3d& transform) const
00148 {
00149   ShapeTransformCache::const_iterator it = transform_cache_.find(h);
00150   if (it == transform_cache_.end())
00151   {
00152     ROS_ERROR("Internal error. Shape filter handle %u not found", h);
00153     return false;
00154   }
00155   transform = it->second;
00156   return true;
00157 }
00158 
00159 void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin,
00160                                           std::vector<int>& mask)
00161 {
00162 }
00163 
00164 void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00165 {
00166   ROS_DEBUG("Received a new point cloud message");
00167   ros::WallTime start = ros::WallTime::now();
00168 
00169   if (monitor_->getMapFrame().empty())
00170     monitor_->setMapFrame(cloud_msg->header.frame_id);
00171 
00172   /* get transform for cloud into map frame */
00173   tf::StampedTransform map_H_sensor;
00174   if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
00175     map_H_sensor.setIdentity();
00176   else
00177   {
00178     if (tf_)
00179     {
00180       try
00181       {
00182         tf_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp,
00183                              map_H_sensor);
00184       }
00185       catch (tf::TransformException& ex)
00186       {
00187         ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback");
00188         return;
00189       }
00190     }
00191     else
00192       return;
00193   }
00194 
00195   /* compute sensor origin in map frame */
00196   const tf::Vector3& sensor_origin_tf = map_H_sensor.getOrigin();
00197   octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
00198   Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
00199 
00200   if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
00201   {
00202     ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
00203     return;
00204   }
00205 
00206   /* mask out points on the robot */
00207   shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
00208   updateMask(*cloud_msg, sensor_origin_eigen, mask_);
00209 
00210   octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
00211   boost::scoped_ptr<sensor_msgs::PointCloud2> filtered_cloud;
00212 
00213   // We only use these iterators if we are creating a filtered_cloud for
00214   // publishing. We cannot default construct these, so we use scoped_ptr's
00215   // to defer construction
00216   boost::scoped_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_x;
00217   boost::scoped_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_y;
00218   boost::scoped_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_z;
00219 
00220   if (!filtered_cloud_topic_.empty())
00221   {
00222     filtered_cloud.reset(new sensor_msgs::PointCloud2());
00223     filtered_cloud->header = cloud_msg->header;
00224     sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
00225     pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
00226     pcd_modifier.resize(cloud_msg->width * cloud_msg->height);
00227 
00228     // we have created a filtered_out, so we can create the iterators now
00229     iter_filtered_x.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "x"));
00230     iter_filtered_y.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "y"));
00231     iter_filtered_z.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "z"));
00232   }
00233   size_t filtered_cloud_size = 0;
00234 
00235   tree_->lockRead();
00236 
00237   try
00238   {
00239     /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
00240      * should be occupied */
00241     for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
00242     {
00243       unsigned int row_c = row * cloud_msg->width;
00244       sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
00245       // set iterator to point at start of the current row
00246       pt_iter += row_c;
00247 
00248       for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
00249       {
00250         // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
00251         //  continue;
00252 
00253         /* check for NaN */
00254         if (!isnan(pt_iter[0]) && !isnan(pt_iter[1]) && !isnan(pt_iter[2]))
00255         {
00256           /* transform to map frame */
00257           tf::Vector3 point_tf = map_H_sensor * tf::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
00258 
00259           /* occupied cell at ray endpoint if ray is shorter than max range and this point
00260              isn't on a part of the robot*/
00261           if (mask_[row_c + col] == point_containment_filter::ShapeMask::INSIDE)
00262             model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
00263           else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
00264             clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
00265           else
00266           {
00267             occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
00268             // build list of valid points if we want to publish them
00269             if (filtered_cloud)
00270             {
00271               **iter_filtered_x = pt_iter[0];
00272               **iter_filtered_y = pt_iter[1];
00273               **iter_filtered_z = pt_iter[2];
00274               ++filtered_cloud_size;
00275               ++*iter_filtered_x;
00276               ++*iter_filtered_y;
00277               ++*iter_filtered_z;
00278             }
00279           }
00280         }
00281       }
00282     }
00283 
00284     /* compute the free cells along each ray that ends at an occupied cell */
00285     for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
00286       if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
00287         free_cells.insert(key_ray_.begin(), key_ray_.end());
00288 
00289     /* compute the free cells along each ray that ends at a model cell */
00290     for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
00291       if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
00292         free_cells.insert(key_ray_.begin(), key_ray_.end());
00293 
00294     /* compute the free cells along each ray that ends at a clipped cell */
00295     for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end; ++it)
00296       if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
00297         free_cells.insert(key_ray_.begin(), key_ray_.end());
00298   }
00299   catch (...)
00300   {
00301     tree_->unlockRead();
00302     return;
00303   }
00304 
00305   tree_->unlockRead();
00306 
00307   /* cells that overlap with the model are not occupied */
00308   for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
00309     occupied_cells.erase(*it);
00310 
00311   /* occupied cells are not free */
00312   for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
00313     free_cells.erase(*it);
00314 
00315   tree_->lockWrite();
00316 
00317   try
00318   {
00319     /* mark free cells only if not seen occupied in this cloud */
00320     for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
00321       tree_->updateNode(*it, false);
00322 
00323     /* now mark all occupied cells */
00324     for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
00325       tree_->updateNode(*it, true);
00326 
00327     // set the logodds to the minimum for the cells that are part of the model
00328     const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
00329     for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
00330       tree_->updateNode(*it, lg);
00331   }
00332   catch (...)
00333   {
00334     ROS_ERROR("Internal error while updating octree");
00335   }
00336   tree_->unlockWrite();
00337   ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
00338   tree_->triggerUpdateCallback();
00339 
00340   if (filtered_cloud)
00341   {
00342     sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
00343     pcd_modifier.resize(filtered_cloud_size);
00344     filtered_cloud_publisher_.publish(*filtered_cloud);
00345   }
00346 }
00347 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Jul 24 2017 02:21:13