00001 /* 00002 * filters_demo_node.cpp 00003 * 00004 * Created on: Aug 14, 2017 00005 * Author: Peter Fankhauser 00006 * Institute: ETH Zurich, Robotic Systems Lab 00007 */ 00008 00009 #include "grid_map_demos/FiltersDemo.hpp" 00010 00011 #include <ros/ros.h> 00012 00013 int main(int argc, char** argv) 00014 { 00015 ros::init(argc, argv, "grid_map_filters_demo"); 00016 ros::NodeHandle nodeHandle("~"); 00017 bool success; 00018 grid_map_demos::FiltersDemo filtersDemo(nodeHandle, success); 00019 if (success) ros::spin(); 00020 return 0; 00021 } 00022 00023 00024 00025 00026 00028 // * filters_demo_node.cpp 00029 // * 00030 // * Created on: Aug 14, 2017 00031 // * Author: Peter Fankhauser 00032 // * Institute: ETH Zurich, Robotic Systems Lab 00033 // */ 00034 // 00035 //#include <filters/filter_chain.h> 00036 //#include <grid_map_core/grid_map_core.hpp> 00037 //#include <grid_map_ros/grid_map_ros.hpp> 00038 //#include <grid_map_msgs/GridMap.h> 00039 //#include <ros/ros.h> 00040 // 00041 //using namespace grid_map; 00042 // 00043 //class GridMapFilterChain 00044 // 00045 // 00046 //(const grid_map_msgs::GridMapPtr& message) 00047 //{ 00048 // GridMap inputMap; 00049 // GridMapRosConverter::fromMessage(message, inputMap); 00050 // ROS_INFO("I heard: [%s]", msg->data.c_str()); 00051 //} 00052 // 00053 //int main(int argc, char** argv) 00054 //{ 00055 // ros::init(argc, argv, "grid_map_filters_demo"); 00056 // ros::NodeHandle nodeHandle("~"); 00057 // ros::Subscriber subscriber = nodeHandle.subscribe("grid_map_in", 1000, chatterCallback); 00058 // ros::Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map_out", 1, true); 00059 // 00060 // // Setup filter chain. 00061 // filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap"); 00062 // if (!filterChain.configure("grid_map_filters", nodeHandle)) { 00063 // ROS_ERROR("Could not configure the filter chain!"); 00064 // return false; 00065 // } 00066 // 00067 // // Create input grid map. 00068 // GridMap inputMap({"elevation"}); 00069 // inputMap.setFrameId("map"); 00070 // inputMap.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0)); 00071 // ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", 00072 // inputMap.getLength().x(), inputMap.getLength().y(), 00073 // inputMap.getSize()(0), inputMap.getSize()(1), 00074 // inputMap.getPosition().x(), inputMap.getPosition().y(), inputMap.getFrameId().c_str()); 00075 // inputMap["elevation"].setRandom(); 00076 // inputMap["elevation"] *= 0.01; 00077 // for (grid_map::CircleIterator iterator(inputMap, Position(0.0, 0.0), 0.15); !iterator.isPastEnd(); ++iterator) { 00078 // inputMap.at("elevation", *iterator) = 0.1; 00079 // } 00080 // 00081 // while (nodeHandle.ok()) { 00082 // 00083 // // Publish original grid map. 00084 // inputMap.setTimestamp(ros::Time::now().toNSec()); 00085 // grid_map_msgs::GridMap message; 00086 // GridMapRosConverter::toMessage(inputMap, message); 00087 // publisher.publish(message); 00088 // ros::Duration(1.0).sleep(); 00089 // 00090 // // Apply filter chain. 00091 // GridMap outputMap; 00092 // if (!filterChain.update(inputMap, outputMap)) { 00093 // ROS_ERROR("Could not update the grid map filter chain!"); 00094 // break; 00095 // } 00096 // 00097 // // Publish filtered output grid map. 00098 // outputMap.setTimestamp(ros::Time::now().toNSec()); 00099 // GridMapRosConverter::toMessage(outputMap, message); 00100 // publisher.publish(message); 00101 // ros::Duration(1.0).sleep(); 00102 // } 00103 // 00104 // return 0; 00105 //}