gazebo_octomap_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H

#include <iostream>
#include <math.h>

#include <rotors_gazebo_plugins/common.h>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <octomap/octomap.h>
#include <ros/ros.h>
#include <rotors_comm/Octomap.h>
#include <sdf/sdf.hh>
#include <std_srvs/Empty.h>

namespace gazebo {

class OctomapFromGazeboWorld : public WorldPlugin {
 public:
  OctomapFromGazeboWorld()
      : WorldPlugin(), node_handle_(kDefaultNamespace), octomap_(NULL) {}
  virtual ~OctomapFromGazeboWorld();

 protected:

  void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf);

  bool CheckIfInterest(const ignition::math::Vector3d & central_point,
                       gazebo::physics::RayShapePtr ray,
                       const double leaf_size);

  void FloodFill(const ignition::math::Vector3d & seed_point,
                 const ignition::math::Vector3d & bounding_box_origin,
                 const ignition::math::Vector3d & bounding_box_lengths,
                 const double leaf_size);
  
  void CreateOctomap(const rotors_comm::Octomap::Request& msg);

 private:
  physics::WorldPtr world_;
  ros::NodeHandle node_handle_;
  ros::ServiceServer srv_;
  octomap::OcTree* octomap_;
  ros::Publisher octomap_publisher_;
  bool ServiceCallback(rotors_comm::Octomap::Request& req,
                       rotors_comm::Octomap::Response& res);
};

} // namespace gazebo

#endif  // ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H