00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
00180
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
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
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
00274 GZ_REGISTER_WORLD_PLUGIN(OctomapFromGazeboWorld)
00275
00276 }