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


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