OctomapToGridmapDemo.cpp
Go to the documentation of this file.
1 /*
2  * OctomapToGridmapDemo.cpp
3  *
4  * Created on: May 03, 2017
5  * Author: Jeff Delmerico
6  * Institute: University of Zürich, Robotics and Perception Group
7  */
8 
10 
12 
13 #include <octomap_msgs/Octomap.h>
14 #include <octomap/octomap.h>
16 #include <octomap_msgs/GetOctomap.h>
17 
18 namespace grid_map_demos {
19 
21  : nodeHandle_(nodeHandle),
22  map_(grid_map::GridMap({"elevation"}))
23 {
25  client_ = nodeHandle_.serviceClient<octomap_msgs::GetOctomap>(octomapServiceTopic_);
26  map_.setBasicLayers({"elevation"});
27  gridMapPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
28  octomapPublisher_ = nodeHandle_.advertise<octomap_msgs::Octomap>("octomap", 1, true);
29 }
30 
32 {
33 }
34 
36 {
37  nodeHandle_.param("octomap_service_topic", octomapServiceTopic_, std::string("/octomap_binary"));
38  nodeHandle_.param("min_x", minX_, NAN);
39  nodeHandle_.param("max_x", maxX_, NAN);
40  nodeHandle_.param("min_y", minY_, NAN);
41  nodeHandle_.param("max_y", maxY_, NAN);
42  nodeHandle_.param("min_z", minZ_, NAN);
43  nodeHandle_.param("max_z", maxZ_, NAN);
44  return true;
45 }
46 
48 {
49  octomap_msgs::GetOctomap srv;
50  if (!client_.call(srv)) {
51  ROS_ERROR_STREAM("Failed to call service: " << octomapServiceTopic_);
52  return;
53  }
54 
55  // creating octree
56  octomap::OcTree* octomap = nullptr;
58  if (tree) {
59  octomap = dynamic_cast<octomap::OcTree*>(tree);
60  } else {
61  ROS_ERROR("Failed to call convert Octomap.");
62  return;
63  }
64 
65  grid_map::Position3 min_bound;
66  grid_map::Position3 max_bound;
67  octomap->getMetricMin(min_bound(0), min_bound(1), min_bound(2));
68  octomap->getMetricMax(max_bound(0), max_bound(1), max_bound(2));
69  if(!std::isnan(minX_))
70  min_bound(0) = minX_;
71  if(!std::isnan(maxX_))
72  max_bound(0) = maxX_;
73  if(!std::isnan(minY_))
74  min_bound(1) = minY_;
75  if(!std::isnan(maxY_))
76  max_bound(1) = maxY_;
77  if(!std::isnan(minZ_))
78  min_bound(2) = minZ_;
79  if(!std::isnan(maxZ_))
80  max_bound(2) = maxZ_;
81  bool res = grid_map::GridMapOctomapConverter::fromOctomap(*octomap, "elevation", map_, &min_bound, &max_bound);
82  if (!res) {
83  ROS_ERROR("Failed to call convert Octomap.");
84  return;
85  }
86  map_.setFrameId(srv.response.map.header.frame_id);
87 
88  // Publish as grid map.
89  grid_map_msgs::GridMap gridMapMessage;
91  gridMapPublisher_.publish(gridMapMessage);
92 
93  // Also publish as an octomap msg for visualization
94  octomap_msgs::Octomap octomapMessage;
95  octomap_msgs::fullMapToMsg(*octomap, octomapMessage);
96  octomapMessage.header.frame_id = map_.getFrameId();
97  octomapPublisher_.publish(octomapMessage);
98 }
99 
100 } /* namespace */
OctomapToGridmapDemo(ros::NodeHandle &nodeHandle)
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::ServiceClient client_
Octomap service client.
void publish(const boost::shared_ptr< M > &message) const
void setBasicLayers(const std::vector< std::string > &basicLayers)
float minX_
Bounding box of octomap to convert.
ros::Publisher octomapPublisher_
Octomap publisher.
const std::string & getFrameId() const
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
static octomap::AbstractOcTree * msgToMap(const Octomap &msg)
static bool fromOctomap(const octomap::OcTree &octomap, const std::string &layer, grid_map::GridMap &gridMap, const grid_map::Position3 *minPoint=nullptr, const grid_map::Position3 *maxPoint=nullptr)
bool call(MReq &req, MRes &res)
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
Eigen::Vector3d Position3
ros::Publisher gridMapPublisher_
Grid map publisher.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
virtual void getMetricMin(double &x, double &y, double &z)
tree
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void getMetricMax(double &x, double &y, double &z)
std::string octomapServiceTopic_
Name of the grid map topic.
ros::NodeHandle & nodeHandle_
ROS nodehandle.
void setFrameId(const std::string &frameId)
#define ROS_ERROR_STREAM(args)
grid_map::GridMap map_
Grid map data.
#define ROS_ERROR(...)


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:37