13 #include <octomap_msgs/Octomap.h> 16 #include <octomap_msgs/GetOctomap.h> 21 : nodeHandle_(nodeHandle),
22 map_(
grid_map::GridMap({
"elevation"}))
49 octomap_msgs::GetOctomap srv;
61 ROS_ERROR(
"Failed to call convert Octomap.");
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_))
71 if(!std::isnan(
maxX_))
73 if(!std::isnan(
minY_))
75 if(!std::isnan(
maxY_))
77 if(!std::isnan(
minZ_))
79 if(!std::isnan(
maxZ_))
83 ROS_ERROR(
"Failed to call convert Octomap.");
89 grid_map_msgs::GridMap gridMapMessage;
94 octomap_msgs::Octomap octomapMessage;
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 ¶m_name, T ¶m_val, const T &default_val) const
virtual void getMetricMin(double &x, double &y, double &z)
virtual ~OctomapToGridmapDemo()
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)
void convertAndPublishMap()
grid_map::GridMap map_
Grid map data.