main.hpp

#ifndef MAIN_HPP
#define MAIN_HPP

#include <iostream>
#include <getopt.h>

#include <graphmap/graphmap.hpp>
#include <graphmap/graphManager.hpp>
#include <graphmap/display.hpp>
#include <graphmap/dummyMappingProcess.hpp>
#include <graphmap/inputManager.hpp>
#include <graphmap/outputManager.hpp>

#include <boost/graph/adjacency_list.hpp>

#include "kernel/keyValueFile.hpp"

typedef boost::shared_ptr<jafar::graphmap::DummyMappingProcess> dummy_mapping_process_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::GraphManager> graph_manager_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::display::Viewer> viewer_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::LinkInputManager> link_manager_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::RobotPoseInputManager> robot_pose_manager_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::GlobalRobotPoseOutputManager> global_robot_pose_manager_ptr_t;
typedef boost::shared_ptr<jafar::graphmap::LocalRobotPoseOutputManager> local_robot_pose_manager_ptr_t;

// Program parameters

//enum { iDispQt = 0, iDispGdhe, iRenderAll, iReplay, iDump, iRandSeed, iPause, iVerbose, iMap, iRobot, iCamera, iTrigger, iGps, iSimu, iExport, nIntOpts };
enum { iDispQt = 0, iDispGdhe, iPause, iVerbose, iSimu, nIntOpts };
int intOpts[nIntOpts] = {0};
const int nFirstIntOpt = 0, nLastIntOpt = nIntOpts-1;

enum { sConfigSetup, nStrOpts };
std::string strOpts[nStrOpts];
const int nFirstStrOpt = nIntOpts, nLastStrOpt = nIntOpts+nStrOpts-1;

enum { bHelp = 0, bUsage, nBreakingOpts };
const int nFirstBreakingOpt = nIntOpts+nStrOpts, nLastBreakingOpt = nIntOpts+nStrOpts+nBreakingOpts-1;


struct option long_options[] = {
  // int options
  {"disp-2d", optional_argument, 0, 0},
  {"disp-3d", optional_argument, 0, 0},
  {"pause", optional_argument, 0, 0},
  {"verbose", optional_argument, 0, 0},
  {"simu", optional_argument, 0, 0},
  // string options
  {"config-setup", required_argument, 0, 0},
  // breaking options
  {"help",no_argument,0,0},
  {"usage",no_argument,0,0},
};

class ConfigSetup: public jafar::kernel::KeyValueFileSaveLoad
{
public:
  unsigned ROBOT_ID; 
  jblas::vec6 ROBOT_POSE; 
private:
  void processKeyValueFile(jafar::kernel::KeyValueFile& keyValueFile, bool read);
public:
  virtual void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
  virtual void saveKeyValueFile(jafar::kernel::KeyValueFile& keyValueFile);
} configSetup;

dummy_mapping_process_ptr_t slamPtr;
graph_manager_ptr_t graphManagerPtr;
viewer_ptr_t viewerPtr;
link_manager_ptr_t linkManagerPtr;
robot_pose_manager_ptr_t robotPoseManagerPtr;
global_robot_pose_manager_ptr_t globalRobotPoseManagerPtr;
local_robot_pose_manager_ptr_t localRobotPoseManagerPtr;

const int display_period = 100; // ms

void demo_graphmap_init()
{
  using namespace jafar;
  using namespace graphmap;

  JFR_GLOBAL_TRY

  if (strOpts[sConfigSetup] == "#!@")
  {
    strOpts[sConfigSetup] = "data/setup.cfg";
  }

  std::cout << "Loading config file " << strOpts[sConfigSetup] << std::endl;
  configSetup.load(strOpts[sConfigSetup]);

  graphManagerPtr.reset(new GraphManager(configSetup.ROBOT_ID));

  linkManagerPtr.reset(new LinkInputManager(graphManagerPtr));
  graphManagerPtr->addInputManager(linkManagerPtr);

  robotPoseManagerPtr.reset(new RobotPoseInputManager(graphManagerPtr));
  graphManagerPtr->addInputManager(robotPoseManagerPtr);

  globalRobotPoseManagerPtr.reset(new GlobalRobotPoseOutputManager(graphManagerPtr));
  graphManagerPtr->addOutputManager(globalRobotPoseManagerPtr);

  localRobotPoseManagerPtr.reset(new LocalRobotPoseOutputManager(graphManagerPtr));
  graphManagerPtr->addOutputManager(localRobotPoseManagerPtr);

  if(intOpts[iSimu]){
    slamPtr.reset(new DummyMappingProcess(configSetup.ROBOT_ID));
  }

#ifndef HAVE_MODULE_QDISPLAY
  intOpts[iDispQt] = 0;
#endif
#ifndef HAVE_MODULE_GDHE
  intOpts[iDispGdhe] = 0;
#endif

  if(intOpts[iDispQt]){
    viewerPtr.reset(new display::Viewer());
  }

//  if(intOpts[iDispGdhe]){

//  }

  switch (intOpts[iVerbose])
  {
    case 0: debug::DebugStream::setLevel("graphmap", debug::DebugStream::Off); break;
    case 1: debug::DebugStream::setLevel("graphmap", debug::DebugStream::Trace); break;
    case 2: debug::DebugStream::setLevel("graphmap", debug::DebugStream::Warning); break;
    case 3: debug::DebugStream::setLevel("graphmap", debug::DebugStream::Debug); break;
    case 4: debug::DebugStream::setLevel("graphmap", debug::DebugStream::VerboseDebug); break;
    default: debug::DebugStream::setLevel("graphmap", debug::DebugStream::VeryVerboseDebug); break;
  }


JFR_GLOBAL_CATCH
}

//void demo_graphmap_display(graph_ptr_t *graph)
void demo_graphmap_display(graph_manager_ptr_t *graph_manager)
{
  if(intOpts[iDispQt] != 0){
    viewerPtr->bufferize((*graph_manager)->_graph);
    viewerPtr->render();
  }
}

//void demo_graphmap_main(graph_ptr_t *graph)
void demo_graphmap_main(graph_manager_ptr_t *graph_manager)
{
  JFR_DEBUG("main function" << std::endl);

  while(!(*graph_manager)->exit()){

    if(intOpts[iSimu]){
      slamPtr->update();
      if(slamPtr->link_poster_updated){
        linkManagerPtr->addInput(slamPtr->link_poster);
        slamPtr->link_poster_updated = false; // to simulate a poster read
      }
      if(slamPtr->robot_pose_poster_updated){
        robotPoseManagerPtr->addInput(slamPtr->robot_pose_poster);
        slamPtr->robot_pose_poster_updated = false; // to simulate a poster read
      }
    }

    /* Use SubMapManager, GpsManager, AbslocManager, etc. All possible modules that add information to the graph would have a manager. Each
     * would write things in a buffer of GraphManager that would be used later in updateGraph(). SubMapManager would also be responsable
     * for telling RT-SLAM (and maybe others also) that a new map was created. We can encapsulate all the RobotId,MapId into these managers,
     * letting it transparent to GraphManager.
     */

    // check for new edges to add in the graph
    graphManagerPtr->updateGraph();

    // optimize loops if any
    graphManagerPtr->optmizeLoops();

    // update frames positions (vertexes data)
    graphManagerPtr->updateVertexData();

    /* Basicaly robot global position, robot local position and current submap ID
     */

  } // temporal-loop

  // dump graph into the memory
  (*graph_manager)->dumpToMemory();

  // drawn a graph in a nice file
  (*graph_manager)->saveGraphImage("demo_graphmap_graph.dot");

}

//void demo_graphmap_exit(graph_ptr_t *graph, boost::thread *thread_main) {
void demo_graphmap_exit(graph_manager_ptr_t *graph_manager, boost::thread *thread_main) {
  JFR_DEBUG("exit function" << std::endl);
  (*graph_manager)->exit(true);
  thread_main->join();
}

void demo_graphmap_run()
{
  using namespace jafar;
  using namespace graphmap;

  JFR_DEBUG("run function" << std::endl);
#ifdef HAVE_MODULE_QDISPLAY
  qdisplay::QtAppStart((qdisplay::FUNC)&demo_graphmap_display, 0,
             (qdisplay::FUNC)&demo_graphmap_main, 0,
             display_period,
             &graphManagerPtr,
             (qdisplay::EXIT_FUNC)&demo_graphmap_exit);
#else
  std::cout << "Please install qdisplay module if you want 2D display" << std::endl;
#endif
}


#define KeyValueFile_processItem(k) { read ? keyValueFile.getItem(#k, k) : keyValueFile.setItem(#k, k); }
#define KeyValueFile_processItem_def(k, def) { read ? keyValueFile.getItem(#k, k, def) : keyValueFile.setItem(#k, k); }

void ConfigSetup::loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile)
{
  jafar::kernel::KeyValueFile &keyValueFile2 = const_cast<jafar::kernel::KeyValueFile&>(keyValueFile);
  processKeyValueFile(keyValueFile2, true);
}

void ConfigSetup::saveKeyValueFile(jafar::kernel::KeyValueFile& keyValueFile)
{
  processKeyValueFile(keyValueFile, false);
}

void ConfigSetup::processKeyValueFile(jafar::kernel::KeyValueFile& keyValueFile, bool read)
{
  KeyValueFile_processItem(ROBOT_ID);
  KeyValueFile_processItem(ROBOT_POSE);
}

#endif // MAIN_HPP