OccupancyGridVisualization.cpp
Go to the documentation of this file.
00001 /*
00002  * OccupancyGridVisualization.cpp
00003  *
00004  *  Created on: Nov 3, 2014
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_visualization/visualizations/OccupancyGridVisualization.hpp"
00010 #include <grid_map_ros/GridMapRosConverter.hpp>
00011 #include <nav_msgs/OccupancyGrid.h>
00012 
00013 namespace grid_map_visualization {
00014 
00015 OccupancyGridVisualization::OccupancyGridVisualization(ros::NodeHandle& nodeHandle, const std::string& name)
00016 : VisualizationBase(nodeHandle, name),
00017   dataMin_(0.0),
00018   dataMax_(1.0)
00019 {
00020 }
00021 
00022 OccupancyGridVisualization::~OccupancyGridVisualization()
00023 {
00024 }
00025 
00026 bool OccupancyGridVisualization::readParameters(XmlRpc::XmlRpcValue& config)
00027 {
00028   VisualizationBase::readParameters(config);
00029 
00030   if (!getParam("layer", layer_)) {
00031     ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str());
00032     return false;
00033   }
00034 
00035   if (!getParam("data_min", dataMin_)) {
00036     ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_min' parameter.", name_.c_str());
00037     return false;
00038   }
00039 
00040   if (!getParam("data_max", dataMax_)) {
00041     ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_max' parameter.", name_.c_str());
00042     return false;
00043   }
00044 
00045   return true;
00046 }
00047 
00048 bool OccupancyGridVisualization::initialize()
00049 {
00050   publisher_ = nodeHandle_.advertise<nav_msgs::OccupancyGrid>(name_, 1, true);
00051   return true;
00052 }
00053 
00054 bool OccupancyGridVisualization::visualize(const grid_map::GridMap& map)
00055 {
00056   if (!isActive()) return true;
00057   if (!map.exists(layer_)) {
00058     ROS_WARN_STREAM("OccupancyGridVisualization::visualize: No grid map layer with name '" << layer_ << "' found.");
00059     return false;
00060   }
00061   nav_msgs::OccupancyGrid occupancyGrid;
00062   grid_map::GridMapRosConverter::toOccupancyGrid(map, layer_, dataMin_, dataMax_, occupancyGrid);
00063   publisher_.publish(occupancyGrid);
00064   return true;
00065 }
00066 
00067 } /* namespace */


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:50