00001 00004 /***************************************************************************** 00005 ** Ifdefs 00006 *****************************************************************************/ 00007 00008 #ifndef cost_map_ros_LOADER_HPP_ 00009 #define cost_map_ros_LOADER_HPP_ 00010 00011 /***************************************************************************** 00012 ** Includes 00013 *****************************************************************************/ 00014 00015 #include <cost_map_core/cost_map_core.hpp> 00016 #include <cost_map_msgs/CostMap.h> 00017 #include <mutex> 00018 #include <ros/ros.h> 00019 #include <std_msgs/String.h> 00020 #include <string> 00021 00022 /***************************************************************************** 00023 ** Namespaces 00024 *****************************************************************************/ 00025 00026 namespace cost_map { 00027 00028 /***************************************************************************** 00029 ** Methods 00030 *****************************************************************************/ 00031 00043 void fromImageBundle(const std::string& filename, cost_map::CostMap& cost_map); 00055 void toImageBundle(const std::string& filename, const cost_map::CostMap& cost_map); 00056 00057 // this might be an interesting api to have too 00058 // void addLayerFromImageFile() 00059 // and this one, which is similar to api in grid_map and inputs opencv types directly 00060 // void addLayerFromImage() 00061 00062 /***************************************************************************** 00063 ** Interfaces 00064 *****************************************************************************/ 00065 00071 class LoadImageBundle { 00072 public: 00079 LoadImageBundle(const std::string& image_bundle_location, 00080 const std::string& topic_name="cost_map"); 00081 virtual ~LoadImageBundle() {} 00082 00083 void publish(); 00084 00085 cost_map::CostMapPtr cost_map; 00086 ros::Publisher publisher; 00087 }; 00088 00094 class SaveImageBundle { 00095 public: 00102 SaveImageBundle(const std::string& topic_name, const std::string& yaml_filename="foo.yaml"); 00103 virtual ~SaveImageBundle() {} 00104 00105 00106 std::string yaml_filename; 00107 bool finished; 00108 00109 private: 00110 void _costmapCallback(const cost_map_msgs::CostMap& msg); 00111 00112 ros::Subscriber subscriber_; 00113 std::mutex mutex_; 00114 }; 00115 00116 /***************************************************************************** 00117 ** Trailers 00118 *****************************************************************************/ 00119 00120 } // namespace cost_map 00121 00122 #endif /* cost_map_ros_LOADER_HPP_ */