00001 00004 /***************************************************************************** 00005 ** Ifdefs 00006 *****************************************************************************/ 00007 00008 #ifndef cost_map_demos_UTILITIES_HPP_ 00009 #define cost_map_demos_UTILITIES_HPP_ 00010 00011 /***************************************************************************** 00012 ** Includes 00013 *****************************************************************************/ 00014 00015 #include <atomic> 00016 #include <costmap_2d/costmap_2d_ros.h> 00017 #include <iostream> 00018 #include <tf/transform_broadcaster.h> 00019 #include <tf/transform_listener.h> 00020 #include <thread> 00021 00022 /***************************************************************************** 00023 ** Namespaces 00024 *****************************************************************************/ 00025 00026 namespace cost_map_demos { 00027 00028 /***************************************************************************** 00029 ** Transforms 00030 *****************************************************************************/ 00031 00035 class TransformBroadcaster { 00036 public: 00037 TransformBroadcaster() : shutdown_flag(false) {} 00038 virtual ~TransformBroadcaster(); 00039 void add(const std::string& name, tf::Vector3 origin, const tf::Quaternion& orientation); 00040 00041 void startBroadCastingThread(); 00042 void broadcast(); 00043 void shutdown(); 00044 00045 private: 00046 std::map<std::string, tf::Transform> transforms; 00047 std::thread broadcasting_thread; 00048 std::atomic<bool> shutdown_flag; 00049 }; 00050 00051 /***************************************************************************** 00052 ** Printing 00053 *****************************************************************************/ 00054 00059 void pretty_print(costmap_2d::Costmap2DROS &ros_costmap); 00060 00068 std::ostream& operator <<(std::ostream& stream, costmap_2d::Costmap2DROS& ros_costmap); 00069 00070 /***************************************************************************** 00071 ** Trailers 00072 *****************************************************************************/ 00073 00074 } // namespace cost_map_demos 00075 00076 #endif /* cost_map_demos_UTILITIES_HPP_ */