OctomapROS.h

Go to the documentation of this file.

// $Id$

/*
 * Copyright (c) 2010, A. Hornung, University of Freiburg
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Freiburg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef OCTOMAP_ROS_H_
#define OCTOMAP_ROS_H_


#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include <tf/transform_datatypes.h>

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>

#pragma message("DEPRECATION WARNING: OctomapROS.h is deprecated and will disappear soon. Use the OctoMap library and octomap_ros/conversions.h directly instead.")

namespace octomap {


  template <class OctreeT>
  class OctomapROS {
  public:
    OctomapROS(double resolution) __attribute__ ((deprecated));
    OctomapROS(OctreeT tree) __attribute__ ((deprecated));
    virtual ~OctomapROS() {}

    template <class PointT>
    void insertScan(const sensor_msgs::PointCloud2& scan, const PointT& origin,
        double maxrange=-1., bool pruning=true, bool dirty=false);

    template <class PCLPointT, class PointT>
    void insertScan(const pcl::PointCloud<PCLPointT>& scan, const PointT& origin,
        double maxrange=-1., bool pruning=true, bool dirty=false);

    template <class PCLPointT, class PCLQuaternionT>
    void insertScan(const pcl::PointCloud<PCLPointT>& scan, const PCLPointT& sensor_origin,
        const PCLPointT& frame_origin_trans, const PCLQuaternionT& frame_origin_rot,
        double maxrange=-1., bool pruning=true, bool dirty=false);




    template <class PointT>
    bool castRay(const PointT& origin, const PointT& direction, PointT& end,
        bool ignoreUnknownCells = false, double maxRange = -1.0) const;

    template <class PointT>
    bool insertRay(const PointT& origin, const PointT& end, double maxRange = -1.0);


    template <class PointT>
    typename OctreeT::NodeType* search(const PointT& point) const;

    typename OctreeT::NodeType* search(const tf::Point& point) const;

    OctreeT octree; 

    typedef OctreeT OcTreeType; 
    typedef typename OctreeT::NodeType NodeType; 
  };

  typedef OctomapROS<OcTree> OcTreeROS;



  //
  // Implementation:
  //

  template <class OctreeT>
  OctomapROS<OctreeT>::OctomapROS(double resolution)
      : octree(resolution)
  {

  }



  template <class OctreeT>
  template <class PCLPointT, class PointT>
  void OctomapROS<OctreeT>::insertScan(const pcl::PointCloud<PCLPointT>& scan, const PointT& origin,
      double maxrange, bool pruning, bool dirty){

    Pointcloud pc;
    pointcloudPCLToOctomap(scan, pc);

    octree.insertScan(pc, point3d(origin.x, origin.y, origin.z), maxrange, pruning, dirty);

  }


  template <class OctreeT>
  template <class PCLPointT, class PCLQuaternionT>
  void OctomapROS<OctreeT>::insertScan(const pcl::PointCloud<PCLPointT>& scan, const PCLPointT& sensor_origin,
      const PCLPointT& frame_origin_trans, const PCLQuaternionT& frame_origin_rot, double maxrange, bool pruning, bool dirty)
    {

    Pointcloud pc;
    pointcloudPCLToOctomap(scan, pc);

    pose6d frame_origin;
    frame_origin.trans() = point3d(frame_origin_trans.x, frame_origin_trans.y, frame_origin_trans.z);
    frame_origin.rot() = octomath::Quaternion(frame_origin_rot.w(),
            frame_origin_rot.x(), frame_origin_rot.y(), frame_origin_rot.z());
    octree.insertScan(pc, point3d(sensor_origin.x, sensor_origin.y, sensor_origin.z), frame_origin,
        maxrange, pruning, dirty);

  }


  template <class OctreeT>
  template <class PointT>
  void OctomapROS<OctreeT>::insertScan(const sensor_msgs::PointCloud2& scan, const PointT& origin,
      double maxrange, bool pruning, bool dirty){


    pcl::PointCloud<pcl::PointXYZ> pc;
    pcl::fromROSMsg(scan, pc);
    insertScan(pc, origin, maxrange, pruning, dirty);

  }

  template <class OctreeT>
  template <class PointT>
  bool OctomapROS<OctreeT>::insertRay(const PointT& origin, const PointT& end, double maxRange){

    return octree.insertRay(point3d(origin.x, origin.y, origin.z),
        point3d(end.x, end.y, end.z),maxRange);

  }

  template <class OctreeT>
  template <class PointT>
  bool OctomapROS<OctreeT>::castRay(const PointT& origin, const PointT& direction,
      PointT& end, bool ignoreUnknownCells, double maxRange) const{

    point3d ptEnd;
    bool result =  octree.castRay(point3d(origin.x, origin.y, origin.z), point3d(direction.x, direction.y, direction.z),
        ptEnd, ignoreUnknownCells, maxRange);

    end.x = ptEnd.x();
    end.y = ptEnd.y();
    end.z = ptEnd.z();

    return result;
  }


  template <class OctreeT>
  template <class PointT>
  typename OctreeT::NodeType* OctomapROS<OctreeT>::search(const PointT& point) const{
    return octree.search(point.x, point.y, point.z);
  }

  template <class OctreeT>
  typename OctreeT::NodeType* OctomapROS<OctreeT>::search(const tf::Point& point) const{
    return octree.search(point.x(), point.y(), point.z());
  }

}

#endif /* OCTOMAP_ROS_H_ */