00001 /* 00002 * PointCloudVisualization.cpp 00003 * 00004 * Created on: Sep 11, 2014 00005 * Author: Péter Fankhauser 00006 * Institute: ETH Zurich, Autonomous Systems Lab 00007 */ 00008 00009 #include <grid_map_visualization/visualizations/PointCloudVisualization.hpp> 00010 #include <grid_map_ros/GridMapRosConverter.hpp> 00011 00012 #include <sensor_msgs/PointCloud2.h> 00013 00014 namespace grid_map_visualization { 00015 00016 PointCloudVisualization::PointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name) 00017 : VisualizationBase(nodeHandle, name) 00018 { 00019 } 00020 00021 PointCloudVisualization::~PointCloudVisualization() 00022 { 00023 } 00024 00025 bool PointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) 00026 { 00027 VisualizationBase::readParameters(config); 00028 if (!getParam("layer", layer_)) { 00029 ROS_ERROR("PointCloudVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); 00030 return false; 00031 } 00032 return true; 00033 } 00034 00035 bool PointCloudVisualization::initialize() 00036 { 00037 publisher_ = nodeHandle_.advertise<sensor_msgs::PointCloud2>(name_, 1, true); 00038 return true; 00039 } 00040 00041 bool PointCloudVisualization::visualize(const grid_map::GridMap& map) 00042 { 00043 if (!isActive()) return true; 00044 if (!map.exists(layer_)) { 00045 ROS_WARN_STREAM("PointCloudVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); 00046 return false; 00047 } 00048 sensor_msgs::PointCloud2 pointCloud; 00049 grid_map::GridMapRosConverter::toPointCloud(map, layer_, pointCloud); 00050 publisher_.publish(pointCloud); 00051 return true; 00052 } 00053 00054 } /* namespace */