FiltersDemo.cpp
Go to the documentation of this file.
00001 /*
00002  * FiltersDemo.cpp
00003  *
00004  *  Created on: Aug 16, 2017
00005  *      Author: Peter Fankhauser
00006  *   Institute: ETH Zurich, Robotic Systems Lab
00007  *
00008  */
00009 
00010 #include "grid_map_demos/FiltersDemo.hpp"
00011 
00012 using namespace grid_map;
00013 
00014 namespace grid_map_demos {
00015 
00016 FiltersDemo::FiltersDemo(ros::NodeHandle& nodeHandle, bool& success)
00017     : nodeHandle_(nodeHandle),
00018       filterChain_("grid_map::GridMap")
00019 {
00020   if (!readParameters()) {
00021     success = false;
00022     return;
00023   }
00024 
00025   subscriber_ = nodeHandle_.subscribe(inputTopic_, 1, &FiltersDemo::callback, this);
00026   publisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>(outputTopic_, 1, true);
00027 
00028   // Setup filter chain.
00029   if (!filterChain_.configure(filterChainParametersName_, nodeHandle)) {
00030     ROS_ERROR("Could not configure the filter chain!");
00031     success = false;
00032     return;
00033   }
00034 
00035   success = true;
00036 }
00037 
00038 FiltersDemo::~FiltersDemo()
00039 {
00040 }
00041 
00042 bool FiltersDemo::readParameters()
00043 {
00044   if (!nodeHandle_.getParam("input_topic", inputTopic_)) {
00045     ROS_ERROR("Could not read parameter `input_topic`.");
00046     return false;
00047   }
00048   nodeHandle_.param("output_topic", outputTopic_, std::string("output"));
00049   nodeHandle_.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters"));
00050   return true;
00051 }
00052 
00053 void FiltersDemo::callback(const grid_map_msgs::GridMap& message)
00054 {
00055   // Convert message to map.
00056   GridMap inputMap;
00057   GridMapRosConverter::fromMessage(message, inputMap);
00058 
00059   // Apply filter chain.
00060   grid_map::GridMap outputMap;
00061   if (!filterChain_.update(inputMap, outputMap)) {
00062     ROS_ERROR("Could not update the grid map filter chain!");
00063     return;
00064   }
00065 
00066   // Publish filtered output grid map.
00067   grid_map_msgs::GridMap outputMessage;
00068   GridMapRosConverter::toMessage(outputMap, outputMessage);
00069   publisher_.publish(outputMessage);
00070 }
00071 
00072 } /* namespace */


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:38