$search

OctomapServer.h

Go to the documentation of this file.

/*
 * Copyright (c) 2010-2012, 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.
 */


#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/ColorRGBA.h>
#include <arm_navigation_msgs/CollisionObject.h>
#include <arm_navigation_msgs/CollisionMap.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_srvs/Empty.h>
#include <dynamic_reconfigure/server.h>
#include <octomap_server/OctomapServerConfig.h>

#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>


#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <octomap_ros/OctomapBinary.h>
#include <octomap_ros/GetOctomap.h>
#include <octomap_ros/ClearBBXRegion.h>
#include <octomap_ros/OctomapROS.h>
#include <octomap/OcTreeKey.h>


namespace octomap_server {
class OctomapServer{

public:
  typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;


  OctomapServer(const std::string& filename= "");
  virtual ~OctomapServer();
  bool serviceCallback(octomap_ros::GetOctomap::Request  &req, octomap_ros::GetOctomap::Response &res);
  bool clearBBXSrv(octomap_ros::ClearBBXRegionRequest& req, octomap_ros::ClearBBXRegionRequest& resp);
  bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);

  void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
  bool openFile(const std::string& filename);

protected:
  void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
  void publishMap(const ros::Time& rostime = ros::Time::now()) const;
  void publishAll(const ros::Time& rostime = ros::Time::now());

  virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);

  void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;

  bool isSpeckleNode(const octomap::OcTreeKey& key) const;

  void handlePreNodeTraversal(const ros::Time& rostime);

  void handleNode(const octomap::OcTreeROS::OcTreeType::iterator& it) {};

  void handleOccupiedNode(const octomap::OcTreeROS::OcTreeType::iterator& it);

  void handleFreeNode(const octomap::OcTreeROS::OcTreeType::iterator& it);

  void handlePostNodeTraversal(const ros::Time& rostime);

  std_msgs::ColorRGBA heightMapColor(double h) const;
  ros::NodeHandle m_nh;
  ros::Publisher m_markerPub, m_binaryMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub;
  message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
  tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
  ros::ServiceServer m_octomapService, m_clearBBXService, m_resetService;
  tf::TransformListener m_tfListener;
  dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;

  octomap::OcTreeROS *m_octoMap;
  octomap::KeyRay m_keyRay;  // temp storage for ray casting
  double m_maxRange;
  std::string m_worldFrameId; // the map frame
  std::string m_baseFrameId; // base of the robot for ground plane filtering
  bool m_useHeightMap;
  std_msgs::ColorRGBA m_color;
  double m_colorFactor;

  bool m_latchedTopics;

  double m_res;
  unsigned m_treeDepth;
  unsigned m_maxTreeDepth;
  double m_probHit;
  double m_probMiss;
  double m_thresMin;
  double m_thresMax;

  double m_pointcloudMinZ;
  double m_pointcloudMaxZ;
  double m_occupancyMinZ;
  double m_occupancyMaxZ;
  double m_minSizeX;
  double m_minSizeY;
  bool m_filterSpeckles;

  bool m_filterGroundPlane;
  double m_groundFilterDistance;
  double m_groundFilterAngle;
  double m_groundFilterPlaneDistance;

  // downprojected 2D map:
  nav_msgs::OccupancyGrid m_gridmap;
  bool m_publish2DMap;
  octomap::OcTreeKey m_paddedMinKey;
  unsigned m_multires2DScale;
};
}

---

octomap_server
Author(s): Armin Hornung
autogenerated on Tue Mar 5 12:07:33 2013