Go to the documentation of this file.00001
00002
00003
00004
00005
00006
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
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
00056 GridMap inputMap;
00057 GridMapRosConverter::fromMessage(message, inputMap);
00058
00059
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
00067 grid_map_msgs::GridMap outputMessage;
00068 GridMapRosConverter::toMessage(outputMap, outputMessage);
00069 publisher_.publish(outputMessage);
00070 }
00071
00072 }