00001 /* 00002 * MathExpressionFilter.cpp 00003 * 00004 * Created on: Aug 16, 2017 00005 * Author: Peter Fankhauser 00006 * Institute: ETH Zurich, Robotic Systems Lab 00007 */ 00008 00009 #include "grid_map_filters/MathExpressionFilter.hpp" 00010 00011 #include <grid_map_core/grid_map_core.hpp> 00012 #include <pluginlib/class_list_macros.h> 00013 00014 using namespace filters; 00015 00016 namespace grid_map { 00017 00018 template<typename T> 00019 MathExpressionFilter<T>::MathExpressionFilter() 00020 { 00021 } 00022 00023 template<typename T> 00024 MathExpressionFilter<T>::~MathExpressionFilter() 00025 { 00026 } 00027 00028 template<typename T> 00029 bool MathExpressionFilter<T>::configure() 00030 { 00031 if (!FilterBase<T>::getParam(std::string("expression"), expression_)) { 00032 ROS_ERROR("MathExpressionFilter did not find parameter 'expression'."); 00033 return false; 00034 } 00035 00036 if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) { 00037 ROS_ERROR("MathExpressionFilter did not find parameter 'output_layer'."); 00038 return false; 00039 } 00040 00041 // TODO Can we make caching work with changing shared variable? 00042 // parser_.setCacheExpressions(true); 00043 return true; 00044 } 00045 00046 template<typename T> 00047 bool MathExpressionFilter<T>::update(const T& mapIn, T& mapOut) 00048 { 00049 mapOut = mapIn; 00050 for (const auto& layer : mapOut.getLayers()) { 00051 parser_.var(layer).setShared(mapOut[layer]); 00052 } 00053 EigenLab::Value<Eigen::MatrixXf> result(parser_.eval(expression_)); 00054 mapOut.add(outputLayer_, result.matrix()); 00055 return true; 00056 } 00057 00058 } /* namespace */ 00059 00060 PLUGINLIB_EXPORT_CLASS(grid_map::MathExpressionFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)