#ifndef __SENSOR_CONVERTER_H_INCLUDED_
#define __SENSOR_CONVERTER_H_INCLUDED_
#include <ros/ros.h>
#include <robot/robot.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>
#include <sensor_msgs/LaserScan.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <move_base_core/navigation.h>
#include <memory>

namespace amr_control
{
  class SensorConverter
  {
  public:
    SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
    virtual ~SensorConverter();

    /**
     * @brief Request map from map server service or wait for message
     * @param nh NodeHandle to use for service client and topic wait
     */
     void requestMapFromServer(ros::NodeHandle &nh);
     
  private:
    /**
     * @brief Callback for map server topic - receives static map and adds it to move_base
     * @param msg OccupancyGrid message from map server
     */
    void mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);

    /**
     * @brief Callback for front laser scan topic - receives and processes front laser data
     * @param msg LaserScan message from front laser sensor
     */
    void laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);

    /**
     * @brief Callback for back laser scan topic - receives and processes back laser data
     * @param msg LaserScan message from back laser sensor
     */
    void laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);

    /**
     * @brief Convert nav_msgs::OccupancyGrid to robot_nav_msgs::OccupancyGrid and add to move_base
     * @param nav_map The nav_msgs map to convert
     */
    void convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map);

    ros::NodeHandle nh_;
    std::shared_ptr<robot::move_base_core::BaseNavigation> move_base_ptr_;
    ros::Subscriber map_server_sub_;
    ros::Subscriber laser_f_scan_sub_;
    ros::Subscriber laser_b_scan_sub_;
  };
}
#endif