octomap_ros: octomap_ros: OctomapROS.h Source File

00001 // $Id: OctomapROS.h 1669 2011-06-27 15:58:35Z hornunga@informatik.uni-freiburg.de $
00002 
00011 /*
00012  * Copyright (c) 2010, A. Hornung, University of Freiburg
00013  * All rights reserved.
00014  *
00015  * Redistribution and use in source and binary forms, with or without
00016  * modification, are permitted provided that the following conditions are met:
00017  *
00018  *     * Redistributions of source code must retain the above copyright
00019  *       notice, this list of conditions and the following disclaimer.
00020  *     * Redistributions in binary form must reproduce the above copyright
00021  *       notice, this list of conditions and the following disclaimer in the
00022  *       documentation and/or other materials provided with the distribution.
00023  *     * Neither the name of the University of Freiburg nor the names of its
00024  *       contributors may be used to endorse or promote products derived from
00025  *       this software without specific prior written permission.
00026  *
00027  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00028  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00029  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00030  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00031  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00032  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00033  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00034  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00035  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00036  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037  * POSSIBILITY OF SUCH DAMAGE.
00038  */
00039 
00040 #ifndef OCTOMAP_ROS_H_
00041 #define OCTOMAP_ROS_H_
00042 
00043 
00044 #include <sensor_msgs/PointCloud2.h>
00045 #include <pcl/point_cloud.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/ros/conversions.h>
00048 #include <tf/transform_datatypes.h>
00049 
00050 #include <geometry_msgs/Point.h>
00051 #include <geometry_msgs/Vector3.h>
00052 #include <octomap/octomap.h>
00053 #include <octomap_ros/conversions.h>
00054 
00055 namespace octomap {
00056 
00057 
00066   template <class OctreeT>
00067   class OctomapROS {
00068   public:
00069     OctomapROS(double resolution);
00070     OctomapROS(OctreeT tree);
00071     virtual ~OctomapROS() {}
00072 
00083     template <class PointT>
00084     void insertScan(const sensor_msgs::PointCloud2& scan, const PointT& origin,
00085         double maxrange=-1., bool pruning=true, bool dirty=false);
00086 
00096     template <class PCLPointT, class PointT>
00097     void insertScan(const pcl::PointCloud<PCLPointT>& scan, const PointT& origin,
00098         double maxrange=-1., bool pruning=true, bool dirty=false);
00099 
00111     template <class PCLPointT, class PCLQuaternionT>
00112     void insertScan(const pcl::PointCloud<PCLPointT>& scan, const PCLPointT& sensor_origin,
00113         const PCLPointT& frame_origin_trans, const PCLQuaternionT& frame_origin_rot,
00114         double maxrange=-1., bool pruning=true, bool dirty=false);
00115 
00116 
00117 
00118 
00133     template <class PointT>
00134     bool castRay(const PointT& origin, const PointT& direction, PointT& end,
00135         bool ignoreUnknownCells = false, double maxRange = -1.0) const;
00136 
00147     template <class PointT>
00148     bool insertRay(const PointT& origin, const PointT& end, double maxRange = -1.0);
00149 
00150 
00158     template <class PointT>
00159     typename OctreeT::NodeType* search(const PointT& point) const;
00160 
00161     typename OctreeT::NodeType* search(const tf::Point& point) const;
00162 
00163     OctreeT octree; 
00164 
00165     typedef OctreeT OcTreeType; 
00166     typedef typename OctreeT::NodeType NodeType; 
00167   };
00168 
00175   typedef OctomapROS<OcTree> OcTreeROS;
00176 
00177 
00178 
00179   //
00180   // Implementation:
00181   //
00182 
00183   template <class OctreeT>
00184   OctomapROS<OctreeT>::OctomapROS(double resolution)
00185       : octree(resolution)
00186   {
00187 
00188   }
00189 
00190 
00191 
00192   template <class OctreeT>
00193   template <class PCLPointT, class PointT>
00194   void OctomapROS<OctreeT>::insertScan(const pcl::PointCloud<PCLPointT>& scan, const PointT& origin,
00195       double maxrange, bool pruning, bool dirty){
00196 
00197     Pointcloud pc;
00198     pointcloudPCLToOctomap(scan, pc);
00199 
00200     octree.insertScan(pc, point3d(origin.x, origin.y, origin.z), maxrange, pruning, dirty);
00201 
00202   }
00203 
00204 
00205   template <class OctreeT>
00206   template <class PCLPointT, class PCLQuaternionT>
00207   void OctomapROS<OctreeT>::insertScan(const pcl::PointCloud<PCLPointT>& scan, const PCLPointT& sensor_origin,
00208       const PCLPointT& frame_origin_trans, const PCLQuaternionT& frame_origin_rot, double maxrange, bool pruning, bool dirty)
00209     {
00210 
00211     Pointcloud pc;
00212     pointcloudPCLToOctomap(scan, pc);
00213 
00214     pose6d frame_origin;
00215     frame_origin.trans() = point3d(frame_origin_trans.x, frame_origin_trans.y, frame_origin_trans.z);
00216     frame_origin.rot() = octomath::Quaternion(frame_origin_rot.w(),
00217             frame_origin_rot.x(), frame_origin_rot.y(), frame_origin_rot.z());
00218     octree.insertScan(pc, point3d(sensor_origin.x, sensor_origin.y, sensor_origin.z), frame_origin,
00219         maxrange, pruning, dirty);
00220 
00221   }
00222 
00223 
00224   template <class OctreeT>
00225   template <class PointT>
00226   void OctomapROS<OctreeT>::insertScan(const sensor_msgs::PointCloud2& scan, const PointT& origin,
00227       double maxrange, bool pruning, bool dirty){
00228 
00229 
00230     pcl::PointCloud<pcl::PointXYZ> pc;
00231     pcl::fromROSMsg(scan, pc);
00232     insertScan(pc, origin, maxrange, pruning, dirty);
00233 
00234   }
00235 
00236   template <class OctreeT>
00237   template <class PointT>
00238   bool OctomapROS<OctreeT>::insertRay(const PointT& origin, const PointT& end, double maxRange){
00239 
00240     return octree.insertRay(point3d(origin.x, origin.y, origin.z),
00241         point3d(end.x, end.y, end.z),maxRange);
00242 
00243   }
00244 
00245   template <class OctreeT>
00246   template <class PointT>
00247   bool OctomapROS<OctreeT>::castRay(const PointT& origin, const PointT& direction,
00248       PointT& end, bool ignoreUnknownCells, double maxRange) const{
00249 
00250     point3d ptEnd;
00251     bool result =  octree.castRay(point3d(origin.x, origin.y, origin.z), point3d(direction.x, direction.y, direction.z),
00252         ptEnd, ignoreUnknownCells, maxRange);
00253 
00254     end.x = ptEnd.x();
00255     end.y = ptEnd.y();
00256     end.z = ptEnd.z();
00257 
00258     return result;
00259   }
00260 
00261 
00262   template <class OctreeT>
00263   template <class PointT>
00264   typename OctreeT::NodeType* OctomapROS<OctreeT>::search(const PointT& point) const{
00265     return octree.search(point.x, point.y, point.z);
00266   }
00267 
00268   template <class OctreeT>
00269   typename OctreeT::NodeType* OctomapROS<OctreeT>::search(const tf::Point& point) const{
00270     return octree.search(point.x(), point.y(), point.z());
00271   }
00272 
00273 }
00274 
00275 #endif /* OCTOMAP_ROS_H_ */