utilities.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef cost_map_demos_UTILITIES_HPP_
#define cost_map_demos_UTILITIES_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <atomic>
#include <costmap_2d/costmap_2d_ros.h>
#include <iostream>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <thread>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cost_map_demos {

/*****************************************************************************
** Transforms
*****************************************************************************/

class TransformBroadcaster {
public:
  TransformBroadcaster() : shutdown_flag(false) {}
  virtual ~TransformBroadcaster();
  void add(const std::string& name, tf::Vector3 origin, const tf::Quaternion& orientation);

  void startBroadCastingThread();
  void broadcast();
  void shutdown();

private:
  std::map<std::string, tf::Transform> transforms;
  std::thread broadcasting_thread;
  std::atomic<bool> shutdown_flag;
};

/*****************************************************************************
** Printing
*****************************************************************************/

void pretty_print(costmap_2d::Costmap2DROS &ros_costmap);

std::ostream& operator <<(std::ostream& stream, costmap_2d::Costmap2DROS& ros_costmap);

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace cost_map_demos

#endif /* cost_map_demos_UTILITIES_HPP_ */