gazebo_octomap_plugin.cpp
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  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
00019  */
00020 
00021 #include "rotors_gazebo_plugins/gazebo_octomap_plugin.h"
00022 
00023 #include <octomap_msgs/conversions.h>
00024 #include <gazebo/common/Time.hh>
00025 #include <gazebo/common/CommonTypes.hh>
00026 #include <gazebo/math/Vector3.hh>
00027 
00028 namespace gazebo {
00029 
00030 OctomapFromGazeboWorld::~OctomapFromGazeboWorld() {
00031   delete octomap_;
00032   octomap_ = NULL;
00033 }
00034 
00035 void OctomapFromGazeboWorld::Load(physics::WorldPtr _parent,
00036                                   sdf::ElementPtr _sdf) {
00037   if (kPrintOnPluginLoad) {
00038     gzdbg << __FUNCTION__ << "() called." << std::endl;
00039   }
00040 
00041   world_ = _parent;
00042 
00043   std::string service_name = "world/get_octomap";
00044   std::string octomap_pub_topic = "world/octomap";
00045   getSdfParam<std::string>(_sdf, "octomapPubTopic", octomap_pub_topic,
00046                            octomap_pub_topic);
00047   getSdfParam<std::string>(_sdf, "octomapServiceName", service_name,
00048                            service_name);
00049 
00050   gzlog << "Advertising service: " << service_name << std::endl;
00051   srv_ = node_handle_.advertiseService(
00052       service_name, &OctomapFromGazeboWorld::ServiceCallback, this);
00053   octomap_publisher_ =
00054       node_handle_.advertise<octomap_msgs::Octomap>(octomap_pub_topic, 1, true);
00055 }
00056 
00057 bool OctomapFromGazeboWorld::ServiceCallback(
00058     rotors_comm::Octomap::Request& req, rotors_comm::Octomap::Response& res) {
00059   gzlog << "Creating octomap with origin at (" << req.bounding_box_origin.x
00060         << ", " << req.bounding_box_origin.y << ", "
00061         << req.bounding_box_origin.z << "), and bounding box lengths ("
00062         << req.bounding_box_lengths.x << ", " << req.bounding_box_lengths.y
00063         << ", " << req.bounding_box_lengths.z
00064         << "), and leaf size: " << req.leaf_size << ".\n";
00065   CreateOctomap(req);
00066   if (req.filename != "") {
00067     if (octomap_) {
00068       std::string path = req.filename;
00069       octomap_->writeBinary(path);
00070       gzlog << std::endl << "Octree saved as " << path << std::endl;
00071     } else {
00072       ROS_ERROR("The octree is NULL. Will not save that.");
00073     }
00074   }
00075   common::Time now = world_->GetSimTime();
00076   res.map.header.frame_id = "world";
00077   res.map.header.stamp = ros::Time(now.sec, now.nsec);
00078 
00079   if (!octomap_msgs::binaryMapToMsg(*octomap_, res.map)) {
00080     ROS_ERROR("Error serializing OctoMap");
00081   }
00082 
00083   if (req.publish_octomap) {
00084     gzlog << "Publishing Octomap." << std::endl;
00085     octomap_publisher_.publish(res.map);
00086   }
00087 
00088   common::SphericalCoordinatesPtr sphericalCoordinates = world_->GetSphericalCoordinates();
00089   ignition::ignition::math::Vector3d origin_cartesian(0.0, 0.0, 0.0);
00090   ignition::ignition::math::Vector3d origin_spherical = sphericalCoordinates->
00091       SphericalFromLocal(origin_cartesian);
00092 
00093   res.origin_latitude = origin_spherical.X();
00094   res.origin_longitude = origin_spherical.Y();
00095   res.origin_altitude = origin_spherical.Z();
00096   return true;
00097 }
00098 
00099 void OctomapFromGazeboWorld::FloodFill(
00100     const ignition::math::Vector3d & seed_point, const ignition::math::Vector3d & bounding_box_origin,
00101     const ignition::math::Vector3d & bounding_box_lengths, const double leaf_size) {
00102   octomap::OcTreeNode* seed =
00103       octomap_->search(seed_point.x, seed_point.y, seed_point.z);
00104   // do nothing if point occupied
00105   if (seed != NULL && seed->getOccupancy()) return;
00106 
00107   std::stack<octoignition::math::Vector3d > to_check;
00108   to_check.push(octoignition::math::Vector3d (seed_point.x, seed_point.y, seed_point.z));
00109 
00110   while (to_check.size() > 0) {
00111     octoignition::math::Vector3d p = to_check.top();
00112 
00113     if ((p.x() > bounding_box_origin.x - bounding_box_lengths.x / 2) &&
00114         (p.x() < bounding_box_origin.x + bounding_box_lengths.x / 2) &&
00115         (p.y() > bounding_box_origin.y - bounding_box_lengths.y / 2) &&
00116         (p.y() < bounding_box_origin.y + bounding_box_lengths.y / 2) &&
00117         (p.z() > bounding_box_origin.z - bounding_box_lengths.z / 2) &&
00118         (p.z() < bounding_box_origin.z + bounding_box_lengths.z / 2) &&
00119         (!octomap_->search(p))) {
00120       octomap_->setNodeValue(p, 0);
00121       to_check.pop();
00122       to_check.push(octoignition::math::Vector3d (p.x() + leaf_size, p.y(), p.z()));
00123       to_check.push(octoignition::math::Vector3d (p.x() - leaf_size, p.y(), p.z()));
00124       to_check.push(octoignition::math::Vector3d (p.x(), p.y() + leaf_size, p.z()));
00125       to_check.push(octoignition::math::Vector3d (p.x(), p.y() - leaf_size, p.z()));
00126       to_check.push(octoignition::math::Vector3d (p.x(), p.y(), p.z() + leaf_size));
00127       to_check.push(octoignition::math::Vector3d (p.x(), p.y(), p.z() - leaf_size));
00128 
00129     } else {
00130       to_check.pop();
00131     }
00132   }
00133 }
00134 
00135 bool OctomapFromGazeboWorld::CheckIfInterest(const ignition::math::Vector3d & central_point,
00136                                              gazebo::physics::RayShapePtr ray,
00137                                              const double leaf_size) {
00138   ignition::math::Vector3d start_point = central_point;
00139   ignition::math::Vector3d end_point = central_point;
00140 
00141   double dist;
00142   std::string entity_name;
00143 
00144   start_point.x += leaf_size / 2;
00145   end_point.x -= leaf_size / 2;
00146   ray->SetPoints(start_point, end_point);
00147   ray->GetIntersection(dist, entity_name);
00148 
00149   if (dist <= leaf_size) return true;
00150 
00151   start_point = central_point;
00152   end_point = central_point;
00153   start_point.y += leaf_size / 2;
00154   end_point.y -= leaf_size / 2;
00155   ray->SetPoints(start_point, end_point);
00156   ray->GetIntersection(dist, entity_name);
00157 
00158   if (dist <= leaf_size) return true;
00159 
00160   start_point = central_point;
00161   end_point = central_point;
00162   start_point.z += leaf_size / 2;
00163   end_point.z -= leaf_size / 2;
00164   ray->SetPoints(start_point, end_point);
00165   ray->GetIntersection(dist, entity_name);
00166 
00167   if (dist <= leaf_size) return true;
00168 
00169   return false;
00170 }
00171 
00172 void OctomapFromGazeboWorld::CreateOctomap(
00173     const rotors_comm::Octomap::Request& msg) {
00174   const double epsilon = 0.00001;
00175   const int far_away = 100000;
00176   ignition::math::Vector3d bounding_box_origin(msg.bounding_box_origin.x,
00177                                     msg.bounding_box_origin.y,
00178                                     msg.bounding_box_origin.z);
00179   // epsilion prevents undefiened behaviour if a point is inserted exactly
00180   // between two octomap cells
00181   ignition::math::Vector3d bounding_box_lengths(msg.bounding_box_lengths.x + epsilon,
00182                                      msg.bounding_box_lengths.y + epsilon,
00183                                      msg.bounding_box_lengths.z + epsilon);
00184   double leaf_size = msg.leaf_size;
00185   octomap_ = new octomap::OcTree(leaf_size);
00186   octomap_->clear();
00187   octomap_->setProbHit(0.7);
00188   octomap_->setProbMiss(0.4);
00189   octomap_->setClampingThresMin(0.12);
00190   octomap_->setClampingThresMax(0.97);
00191   octomap_->setOccupancyThres(0.7);
00192 
00193   gazebo::physics::PhysicsEnginePtr engine = world_->PhysicsEngine();
00194   engine->InitForThread();
00195   gazebo::physics::RayShapePtr ray =
00196       boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
00197           engine->CreateShape("ray", gazebo::physics::CollisionPtr()));
00198 
00199   std::cout << "Rasterizing world and checking collisions" << std::endl;
00200 
00201   for (double x =
00202            leaf_size / 2 + bounding_box_origin.x - bounding_box_lengths.x / 2;
00203        x < bounding_box_origin.x + bounding_box_lengths.x / 2; x += leaf_size) {
00204     int progress =
00205         round(100 * (x + bounding_box_lengths.x / 2 - bounding_box_origin.x) /
00206               bounding_box_lengths.x);
00207     std::cout << "\rPlacing model edges into octomap... " << progress
00208               << "%                 ";
00209 
00210     for (double y =
00211              leaf_size / 2 + bounding_box_origin.y - bounding_box_lengths.y / 2;
00212          y < bounding_box_origin.y + bounding_box_lengths.y / 2;
00213          y += leaf_size) {
00214       for (double z = leaf_size / 2 + bounding_box_origin.z -
00215                       bounding_box_lengths.z / 2;
00216            z < bounding_box_origin.z + bounding_box_lengths.z / 2;
00217            z += leaf_size) {
00218         ignition::math::Vector3d point(x, y, z);
00219         if (CheckIfInterest(point, ray, leaf_size)) {
00220           octomap_->setNodeValue(x, y, z, 1);
00221         }
00222       }
00223     }
00224   }
00225   octomap_->prune();
00226   octomap_->updateInnerOccupancy();
00227 
00228   // flood fill from top and bottom
00229   std::cout << "\rFlood filling freespace...                                  ";
00230   FloodFill(ignition::math::Vector3d (bounding_box_origin.x + leaf_size / 2,
00231                           bounding_box_origin.y + leaf_size / 2,
00232                           bounding_box_origin.z + bounding_box_lengths.z / 2 -
00233                               leaf_size / 2),
00234             bounding_box_origin, bounding_box_lengths, leaf_size);
00235   FloodFill(ignition::math::Vector3d (bounding_box_origin.x + leaf_size / 2,
00236                           bounding_box_origin.y + leaf_size / 2,
00237                           bounding_box_origin.z - bounding_box_lengths.z / 2 +
00238                               leaf_size / 2),
00239             bounding_box_origin, bounding_box_lengths, leaf_size);
00240 
00241   octomap_->prune();
00242   octomap_->updateInnerOccupancy();
00243 
00244   // set unknown to filled
00245   for (double x =
00246            leaf_size / 2 + bounding_box_origin.x - bounding_box_lengths.x / 2;
00247        x < bounding_box_origin.x + bounding_box_lengths.x / 2; x += leaf_size) {
00248     int progress =
00249         round(100 * (x + bounding_box_lengths.x / 2 - bounding_box_origin.x) /
00250               bounding_box_lengths.x);
00251     std::cout << "\rFilling closed spaces... " << progress << "%              ";
00252 
00253     for (double y =
00254              leaf_size / 2 + bounding_box_origin.y - bounding_box_lengths.y / 2;
00255          y < bounding_box_origin.y + bounding_box_lengths.y / 2;
00256          y += leaf_size) {
00257       for (double z = leaf_size / 2 + bounding_box_origin.z -
00258                       bounding_box_lengths.z / 2;
00259            z < bounding_box_origin.z + bounding_box_lengths.z / 2;
00260            z += leaf_size) {
00261         octomap::OcTreeNode* seed = octomap_->search(x, y, z);
00262         if (!seed) octomap_->setNodeValue(x, y, z, 1);
00263       }
00264     }
00265   }
00266 
00267   octomap_->prune();
00268   octomap_->updateInnerOccupancy();
00269 
00270   std::cout << "\rOctomap generation completed                  " << std::endl;
00271 }
00272 
00273 // Register this plugin with the simulator
00274 GZ_REGISTER_WORLD_PLUGIN(OctomapFromGazeboWorld)
00275 
00276 }  // namespace gazebo


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43