00001 #pragma once 00002 00003 /***************************************************************************** 00004 ** Includes 00005 *****************************************************************************/ 00006 00007 #include <costmap_2d/costmap_2d_ros.h> 00008 #include <grid_map_core/grid_map_core.hpp> 00009 #include <grid_map_costmap_2d/grid_map_costmap_2d.hpp> 00010 #include <tf/transform_listener.h> 00011 #include <tf/transform_broadcaster.h> 00012 00013 #include <atomic> 00014 #include <map> 00015 #include <string> 00016 #include <thread> 00017 00018 /***************************************************************************** 00019 ** Transforms 00020 *****************************************************************************/ 00021 00025 class TransformBroadcaster { 00026 public: 00027 TransformBroadcaster() : shutdownFlag_(false) {} 00028 virtual ~TransformBroadcaster(); 00029 void add(const std::string& name, tf::Vector3 origin, const tf::Quaternion& orientation); 00030 00031 void startBroadCastingThread(); 00032 void broadcast(); 00033 void shutdown(); 00034 00035 private: 00036 std::map<std::string, tf::Transform> transforms_; 00037 std::thread broadcastingThread_; 00038 std::atomic<bool> shutdownFlag_; 00039 }; 00040 00051 class ROSCostmapServer { 00052 public: 00053 typedef costmap_2d::Costmap2DROS ROSCostmap; 00054 typedef std::shared_ptr<ROSCostmap> ROSCostmapPtr; 00055 00056 ROSCostmapServer(const std::string& name, const std::string& baseLinkTransformName, 00057 const grid_map::Position& origin, const double& width, const double& height); 00058 00059 ROSCostmapPtr getROSCostmap() { return costmap; }; 00060 00061 private: 00062 ROSCostmapPtr costmap; 00063 tf::TransformListener transformListener; 00064 }; 00065 00066 00067 /***************************************************************************** 00068 ** Helpers 00069 *****************************************************************************/ 00070 00077 void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster);