optimizer_ros.cpp

Go to the documentation of this file.

#include <cstdio>
#include <iostream>
#include <cstring>
#include <fstream> 
 
#include <optimizer_ros.h>

using namespace robotLibPbD;

 
std::string COptimizerRos::generateOutput()
{
  CFrameInterface interface;
  std::string text;

  text = "<launch>\n";

  unsigned int counter = 0;
  for (unsigned int i=0; i<frames.getFrames().size(); i++)
    {
      interface.setFrame(frames.getFrame(i));
      LOG_VERBOSE("Frame: %s\n", interface.getFrameAsXml().c_str());

      CVec position, quater;
      position = frames.getFrame(i)->getPose()[3];
      CMathLib::quaternionFromMatrix(frames.getFrame(i)->getPose(), quater); 
 
      if (frames.getFrame(i)->isData())
        {
          text += printToString("# input: %s\n", frames.getFrame(i)->getName());
        } else
        {
          if (frames.getFrame(i)->getBase() == NULL && frames.getFrame(i)->getRelativeToBase().length() < 1.0e-10)
            continue;

          text += printToString("<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"%s\" args=\"%f %f %f %f %f %f %f %s %s 100\"/> \n",
                                printToString("calibration_publisher_%d", counter).c_str(),
                                position.x / 1000.0, position.y / 1000.0, position.z / 1000.0,
                                quater.x, quater.y, quater.z, quater.w,
                                frames.getFrame(i)->getBase() == NULL ? "CalibrationBase" : frames.getFrame(i)->getBase()->getName(),
                                frames.getFrame(i)->getName());
                                counter++;
        }
    }

  text += "</launch>\n";

  return text;
}