OctomapToGridmapDemo.cpp
Go to the documentation of this file.
00001 /*
00002  * OctomapToGridmapDemo.cpp
00003  *
00004  *  Created on: May 03, 2017
00005  *      Author: Jeff Delmerico
00006  *   Institute: University of Zürich, Robotics and Perception Group
00007  */
00008 
00009 #include "grid_map_demos/OctomapToGridmapDemo.hpp"
00010 
00011 #include <grid_map_octomap/GridMapOctomapConverter.hpp>
00012 
00013 #include <octomap_msgs/Octomap.h>
00014 #include <octomap/octomap.h>
00015 #include <octomap_msgs/conversions.h>
00016 #include <octomap_msgs/GetOctomap.h>
00017 
00018 namespace grid_map_demos {
00019 
00020 OctomapToGridmapDemo::OctomapToGridmapDemo(ros::NodeHandle& nodeHandle)
00021     : nodeHandle_(nodeHandle),
00022       map_(grid_map::GridMap({"elevation"}))
00023 {
00024   readParameters();
00025   client_ = nodeHandle_.serviceClient<octomap_msgs::GetOctomap>(octomapServiceTopic_);
00026   map_.setBasicLayers({"elevation"});
00027   gridMapPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00028   octomapPublisher_ = nodeHandle_.advertise<octomap_msgs::Octomap>("octomap", 1, true);
00029 }
00030 
00031 OctomapToGridmapDemo::~OctomapToGridmapDemo()
00032 {
00033 }
00034 
00035 bool OctomapToGridmapDemo::readParameters()
00036 {
00037   nodeHandle_.param("octomap_service_topic", octomapServiceTopic_, std::string("/octomap_binary"));
00038   nodeHandle_.param("min_x", minX_, NAN);
00039   nodeHandle_.param("max_x", maxX_, NAN);
00040   nodeHandle_.param("min_y", minY_, NAN);
00041   nodeHandle_.param("max_y", maxY_, NAN);
00042   nodeHandle_.param("min_z", minZ_, NAN);
00043   nodeHandle_.param("max_z", maxZ_, NAN);
00044   return true;
00045 }
00046 
00047 void OctomapToGridmapDemo::convertAndPublishMap()
00048 {
00049   octomap_msgs::GetOctomap srv;
00050   if (!client_.call(srv)) {
00051     ROS_ERROR_STREAM("Failed to call service: " << octomapServiceTopic_);
00052     return;
00053   }
00054 
00055   // creating octree
00056   octomap::OcTree* octomap = nullptr;
00057   octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(srv.response.map);
00058   if (tree) {
00059     octomap = dynamic_cast<octomap::OcTree*>(tree);
00060   } else {
00061     ROS_ERROR("Failed to call convert Octomap.");
00062     return;
00063   }
00064 
00065   grid_map::Position3 min_bound;
00066   grid_map::Position3 max_bound;
00067   octomap->getMetricMin(min_bound(0), min_bound(1), min_bound(2));
00068   octomap->getMetricMax(max_bound(0), max_bound(1), max_bound(2));
00069   if(!std::isnan(minX_))
00070     min_bound(0) = minX_;
00071   if(!std::isnan(maxX_))
00072     max_bound(0) = maxX_;
00073   if(!std::isnan(minY_))
00074     min_bound(1) = minY_;
00075   if(!std::isnan(maxY_))
00076     max_bound(1) = maxY_;
00077   if(!std::isnan(minZ_))
00078     min_bound(2) = minZ_;
00079   if(!std::isnan(maxZ_))
00080     max_bound(2) = maxZ_;
00081   bool res = grid_map::GridMapOctomapConverter::fromOctomap(*octomap, "elevation", map_, &min_bound, &max_bound);
00082   if (!res) {
00083     ROS_ERROR("Failed to call convert Octomap.");
00084     return;
00085   }
00086   map_.setFrameId(srv.response.map.header.frame_id);
00087 
00088   // Publish as grid map.
00089   grid_map_msgs::GridMap gridMapMessage;
00090   grid_map::GridMapRosConverter::toMessage(map_, gridMapMessage);
00091   gridMapPublisher_.publish(gridMapMessage);
00092 
00093   // Also publish as an octomap msg for visualization
00094   octomap_msgs::Octomap octomapMessage;
00095   octomap_msgs::fullMapToMsg(*octomap, octomapMessage);
00096   octomapMessage.header.frame_id = map_.getFrameId();
00097   octomapPublisher_.publish(octomapMessage);
00098 }
00099 
00100 } /* namespace */


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:38