rotors_gazebo_plugins: gazebo_octomap

Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H
00023 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H
00024 
00025 #include <iostream>
00026 #include <math.h>
00027 
00028 #include <rotors_gazebo_plugins/common.h>
00029 #include <gazebo/common/common.hh>
00030 #include <gazebo/gazebo.hh>
00031 #include <gazebo/physics/physics.hh>
00032 #include <octomap/octomap.h>
00033 #include <ros/ros.h>
00034 #include <rotors_comm/Octomap.h>
00035 #include <sdf/sdf.hh>
00036 #include <std_srvs/Empty.h>
00037 
00038 namespace gazebo {
00039 
00043 class OctomapFromGazeboWorld : public WorldPlugin {
00044  public:
00045   OctomapFromGazeboWorld()
00046       : WorldPlugin(), node_handle_(kDefaultNamespace), octomap_(NULL) {}
00047   virtual ~OctomapFromGazeboWorld();
00048 
00049  protected:
00050 
00054   void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf);
00055 
00056   bool CheckIfInterest(const ignition::math::Vector3d & central_point,
00057                        gazebo::physics::RayShapePtr ray,
00058                        const double leaf_size);
00059 
00060   void FloodFill(const ignition::math::Vector3d & seed_point,
00061                  const ignition::math::Vector3d & bounding_box_origin,
00062                  const ignition::math::Vector3d & bounding_box_lengths,
00063                  const double leaf_size);
00064   
00081   void CreateOctomap(const rotors_comm::Octomap::Request& msg);
00082 
00083  private:
00084   physics::WorldPtr world_;
00085   ros::NodeHandle node_handle_;
00086   ros::ServiceServer srv_;
00087   octomap::OcTree* octomap_;
00088   ros::Publisher octomap_publisher_;
00089   bool ServiceCallback(rotors_comm::Octomap::Request& req,
00090                        rotors_comm::Octomap::Response& res);
00091 };
00092 
00093 } // namespace gazebo
00094 
00095 #endif  // ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H